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Abstract.  Using  the  MIT  Serial  Link  Direct  Drive  Arm  as  the  main  experimental 
device,  various  issues  in  trajectory  and  force  control  of  manipulators  were  studied 
in  this  thesis.  Since  accurate  modelling  is  important  for  any  controller,  issues  of 
estimating  the  dynamic  model  of  a  manipulator  and  its  load  were  addressed  first. 
Practical  and  effective  algorithms  were  developed  from  the  Newton-Euler  equations 
to  estimate  the  inertial  parameters  of  manipulator  rigid-body  loads  and  links.  Load 
estimation  was  implemented  both  on  a  PUMA  600  robot  and  on  the  MIT  Serial 
Link  Direct  Drive  Arm.  With  the  link  estimation  algorithm,  the  inertial  param¬ 
eters  of  the  direct  drive  arm  were  obtained.  For  both  load  and  link  estimation 
results,  the  estimated  parameters  are  good  models  of  the  actual  system  for  control 
purposes  since  torques  and  forces  can  be  predicted  accurately  from  these  estimated 
parameters,  ,_j 

''The  estimated  model  of  the  direct  drive  arm  was  then  used  to  evaluate  trajec¬ 
tory  following  performance  by  feedforward  and  computed  torque  control  algorithms. 
The  experimental  evaluations  showed  that  the  dynamic  compensation  can  greatly 
improve  trajectory  following  accuracy, 

Various  stability  issues  of  force  control  were  studied  next.  It  was  determined 
that  there  are  two  types  of  instability  in  force  control.  Dynamic  instability,  present 
in  all  of  the  previous  force  control  algorithms  discussed  in  this  thesis,  is  caused  by 
the  interaction  of  a  manipulator  with  a  stiff  environment.  Kinematic  instability  is 
present  only  in  the  hybrid  control  algorithm  of  Raibert  and  Craig,  and  is  caused 
by  the  interaction  of  the  inertia  matrix  with  the  Jacobian  inverse  coordinate  trans¬ 
formation  in  the  feedback  path.  Several  methods  were  suggested  and  demonstrated 
experimentally  to  solve  these  stability  problems.  The  results  of  the  stability  anal¬ 
yses  were  then  incorporated  in  implementing  a  stable  force/position  controller  on 
the  direct  drive  arm  by  the  modified  resolved  acceleration  method  using  both  joint 
torque  and  wrist  force  sensor  feedbacks. 
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Chapter  1 
Introduction 


The  control  of  robot  manipulators  has  become  an  important  subject  of  study  due 
to  growing  interests  and  uses  of  robot  manipulators.  Unfortunately,  very  few  of  the 
proposed  control  algorithms  have  ever  been  implemented  on  an  actual  manipulator, 
and  it  is  not  clear  how  good  or  practical  these  algorithms  really  are.  This  lack  of 
experimental  results  is  mainly  due  to  the  lack  of  a  high  quality  manipulator  that 
can  benefit  from  sophisticated  control  algorithms.  The  motivation  of  this  thesis 
is  to  evaluate  some  of  these  control  algorithms  (both  trajectory  and  force  control) 
using  a  high  quality  direct  drive  arm  and  to  understand  some  of  the  problems  in 
robot  control  that  researchers  have  observed  in  the  past.  Then,  some  new  methods 
of  control  are  developed  to  overcome  those  problems. 


1.1  Direct  Drive  Arm 

A  typical  industrial  manipulator  such  as  the  PUMA  (Unimation,  Inc.)  has  small 
actuators  at  the  joints  and  utilizes  very  large  gear  ratios  in  order  to  be  able  to 
exert  enough  torque  to  the  links.  This  arrangement  of  actuation  introduces  a  large 
amount  of  undesirable  nonlinearities  such  as  friction  and  backlash  at  the  joints.  In 
fact,  for  the  PUMA  600  manipulator  at  the  MIT  Artificial  Intelligence  Laboratory, 
it  was  measured  that  the  friction  terms  account  for  as  much  as  50%  of  the  motor 
torques.  Ironically,  since  these  nonlinear  effects  are  difficult  to  deal  with,  most  of 
the  proposed  control  algorithms  are  based  on  the  rigid  body  dynamic  model  of  the 
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robot,  neglecting  the  non-ideal  characteristics.  Therefore,  these  algorithms  cannot 
be  used  on  typical  manipulators  effectively.  In  fact,  the  effects  of  the  rigid  body 
dynamics  of  the  links  are  very  small  for  these  highly  geared  manipulators  since 
these  effects  are  reduced  by  the  square  of  the  gear  ratio.  A  typical  gear  ratio  of 
100  :  1  would  then  reduce  the  dynamic  effects  of  the  links  by  10-4.  As  a  result,  the 
dynamics  of  a  conventional  robot  are  dominated  by  the  motor  inertias  and  the  joint 
frictions  (Goor,  1985;  Good,  Sweet,  and  Strobel,  1985).  This  is  the  main  reason 
that  the  most  common  form  of  controller  for  these  manipulators  are  the  independent 
joint  PID  controllers. 

Conventional  robots  have  other  disadvantages.  Because  of  their  characteristics 
mentioned  above,  they  are  in  general  slow,  and  cannot  be  used  in  high  speed  ap¬ 
plications  such  as  laser  cutting  (Youcef-Toumi,  1985).  Also,  they  are  essentially 
positioning  devices  and  are  not  suitable  for  controlling  interaction  forces  at  the 
tip  of  the  manipulator  since  the  actuators  with  high  gear  ratios  cannot  be  used  to 
command  torques  effectively.  Typically  the  best  way  to  control  these  manipulators 
is  to  implement  tight  position  loops  at  the  joints,  thus  making  the  actuators  into 
position  sources. 

Recently,  however,  several  high  quality  direct  drive  arms  have  been  developed  in 
order  to  overcome  some  of  the  performance  limitations  of  the  conventional  robots 
(Asada  and  Kanade,  1981;  Asada,  Kanade  and  Takeyama,  1983;  Asada  and  Youcef- 
Toumi,  1984;  Curran  and  Mayer,  1985;  Kuwahara,  Ono,  Nikaido,  and  Matsumoto, 
1985)  .  Since  the  links  are  directly  coupled  to  the  motors,  the  backlash  effects 
are  eliminated  and  the  joint  frictional  effects  are  reduced  immensely.  Therefore, 
the  dynamics  of  these  direct  drive  arms  are  modelled  accurately  by  the  rigid  body 
dynamics  of  the  links.  This  characteristic  makes  these  manipulators  not  only  more 
suitable  to  test  the  recent  control  algorithms  based  on  link  dynamics,  but  also 
makes  it  necessary  to  use  such  sophisticated  control  algorithms  since  the  full  coupled 
dynamics  of  the  links  are  reflected  directly  to  the  actuators. 

Without  the  high  gearing  and  the  undesirable  nonlinear  effects  at  the  joints, 
the  control  of  joint  torques  also  becomes  more  feasible.  Since  the  actuators  can  be 
treated  as  torque  sources,  they  are  more  suitable  for  controlling  forces  and  torques 
at  the  tip  of  the  manipulator. 

But,  there  are  also  some  drawbacks  with  the  direct  drive  arm  technology.  Since 
there  is  no  gearing  to  amplify  the  motor  torques,  the  motors  have  to  be  large  to 
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be  able  to  exert  large  torques.  This  makes  the  whole  manipulator  large  and  more 
difficult  to  control.  Also,  since  the  motors  have  to  exert  large  torques,  large  currents 
flow  through  the  windings,  overheating  the  motors  quickly.  Another  drawback  is 
that  the  manipulator  dynamics  will  be  sensitive  to  the  changes  in  loads  at  the  tip  of 
the  manipulator,  since  the  load  inertial  effects  are  fully  reflected  to  the  joints  without 
any  reduction  through  the  gears.  Despite  these  drawbacks,  the  ability  to  model  the 
manipulator  accurately  by  the  ideal  rigid  body  dynamics  makes  these  manipulators 
very  attractive  for  control  studies  and  for  high  performance  applications. 

The  main  experimental  device  used  in  this  thesis  is  the  MIT  Serial  Link  Direct 
Drive  Arm  (DDARM)  (Fig.  l.l),  developed  by  Haruhiko  Asada  while  he  was  at 
MIT.  It  is  a  three  link  manipulator  with  a  three  phase  rare-earth  permanent  magnet 
brushless  DC  motor  placed  at  each  joint.  The  serial  link  configuration  of  this 
manipulator  differentiates  this  direct  drive  arm  from  another  direct  drive  arm  at 
MIT,  which  was  also  developed  by  Asada  but  has  parallel  linkages.  The  motor 
charateristics  of  the  DDARM  are  listed  in  Table  1.1  (Youcef-Toumi,  1985). 


motor 

motor 

peak 

rotor 

max.  current 

dia. 

mass 

torque 

inertia 

(Amp) 

(cm) 

(kg) 

(Nm) 

(kg-m2) 

#  poles  instantaneous  continuous 

Joint  1 

35 

20.39 

660 

0.181 

30  50  15 

Joints  2  &  3 

25 

16.5 

230 

0.0256 

18  30  10 

Table  1.1:  Motor  characteristics  for  the  Direct  Drive  Arm 


1.2  Objectives 


As  mentioned  in  the  first  paragraph,  a  goal  of  this  thesis  is  to  study  robot  control 
using  the  DDARM.  The  first  step  in  any  control  design  is  the  accurate  modelling  of 
the  plant  to  be  controlled.  In  practice,  especially  with  the  availability  of  automatic 
control  design  tools,  this  modelling  step  may  occupy  greater  than  90%  of  the  control 
designer’s  efforts.  Hence,  for  controlling  a  direct  drive  arm,  accurate  modelling 
of  the  manipulator  is  important.  Since  the  actuators  are  inherently  parts  of  the 
links  for  direct  drive  arms,  separate  modelling  of  the  mechanical  properties  of  the 
actuators  are  not  necessary.  The  electrical  dynamics  of  the  actuators  are  often 
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orders  of  magnitude  faster  than  the  inertial  dynamics  and  it  may  not  be  necessary 
to  include  them  in  the  robot  model. 

Kinematic  parameters  are  usually  well  known  or  can  be  calibrated  using  methods 
developed  by  Whitney,  Lozinski,  and  Rourke  (1986)  and  others.  However,  the 
inertial  parameters,  i.e.  the  mass,  the  location  of  center  of  mass,  and  the  moments 
of  inertia  of  each  rigid  body  link  of  a  robot  are  usually  not  known  even  to  the 
manufacturers  of  the  robots.  Also,  even  if  the  links  were  modelled  accurately,  the 
inertial  parameters  of  the  loads  vary  with  different  loads.  Since  a  load  is  essentially 
a  part  of  the  last  link,  the  knowledge  of  the  inertial  parameters  of  manipulator 
loads  is  also  important  for  accurate  control  of  manipulators.  Therefore,  one  of 
the  objectives  of  this  thesis  is  to  develop  a  practical  algorithm  to  estimate  the 
inertial  parameters  of  links  and  loads  accurately.  The  second  objective  is  to  use  the 
estimated  parameters  in  the  design  of  trajectory  and  force  control  algorithms  for 
the  direct  drive  arm  and  evaluate  experimental  results  using  different  controllers. 

Between  the  area  of  trajectory  control  and  the  area  of  force  control  of  manipu¬ 
lators,  the  issues  of  force  control  are  much  less  understood.  As  mentioned  before, 
conventional  robots  are  not  well  suited  for  implementating  force  control  algorithms 
since  they  are  essentially  positioning  devices.  Therefore,  previous  implementations 
seldom  produced  satisfactory  results  (Caine,  1985).  In  fact,  researchers  in  the  past 
have  experienced  significant  instability  problems  associated  with  force  controllers 
(Whitney,  1985).  Therefore,  another  goal  of  this  thesis  is  to  understand  some  of 
the  stability  and  performance  problems  associated  with  force  control,  and  suggest 
and  demonstrate  some  remedies  to  those  problems  using  the  direct  drive  arm. 

As  stressed  several  times  already,  the  implementation  aspects  are  important 
parts  of  my  thesis  work  since  very  few  experimental  results  are  available  in  the 
field  of  robot  control.  Experiments  are  important  not  only  because  they  can  verify 
the  theory  but  also  some  practical  problems  and  insights  may  be  discovered  during 
implementations  that  may  not  have  been  obvious  at  first.  This  was  true  throughout 
my  thesis  work. 

1.3  Literature  Survey 

This  section  is  brief  since  each  chapter  of  this  thesis,  except  for  Chapters  1,  7,  and 
8,  contains  introduction  section  which  includes  the  survey  of  previous  works. 
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The  direct  drive  arm  technology  has  been  pioneered  in  the  U.S.  by  Haruhiko 
Asada.  His  first  version  was  developed  at  the  Carnegi e-Mellon  University  (Asada 
and  Kanade,  1981;  Asada,  Kanade,  and  Takeyama,  1983).  At  MIT,  Asada,  Youcef- 
Toumi  (1985),  and  Ro  (Asada  and  Ro,  1985)  developed  the  invariant  inertia  method 
for  designing  direct  drive  arms. 

Since  the  importance  of  modelling  the  manipulator  accurately  has  been  increas¬ 
ingly  apparent  with  the  demand  for  high  performance  controller,  several  investiga¬ 
tors  have  recently  suggested  various  algorithms  for  estimating  the  kinematic  and 
the  dynamic  models.  Whitney  (1986)  has  presented  the  most  comprehensive  work 
so  far  for  calibrating  the  kinematic  parameters  using  measurements  with  theodo¬ 
lites.  His  work  includes  both  theoretical  and  experimental  results.  Other  works 
on  kinematic  identification  are  by  Wu  (1983),  Hayati  (1983),  Mooring  (1983)  ,  and 
Sugimoto  and  Okada  (1984). 

Paul  (1981),  Coiffet  (1983),  Olsen  and  Bekey  (1985),  and  Mukerjee  and  Ballard 
(1985)  have  studied  the  rigid-body  load  identification  problem,  and  Mayeda,  Osuka, 
and  Kangawa  (1984),  Olsen  and  Bekey  (1985),  Mukerjee  and  Ballard  (1985),  New¬ 
man  and  Khosla  (1985)  and  Khalil,  Gautier,  and  Kleinfinger  (1986)  have  studied 
the  rigid-body  link  identification  problem.  Some  of  the  works  by  the  above  authors 
are  similar  to  the  algorithms  presented  in  this  thesis,  but  very  few  have  been  veri¬ 
fied  by  implementation.  In  addition,  Cannon  and  Schmitz  (1984)  and  Book  (1984) 
among  other  researchers  have  considered  modelling  flexible  modes  of  links. 

There  have  been  many  control  algorithms  based  on  the  rigid  body  dynamic 
model  of  a  manipulator.  The  computed  torque  control  method  for  trajectory  control 
has  been  studied  by  various  researchers  (Paul,  1972;  Markiewicz,  1973;  Bejczy, 
1974;  Luh,  Walker,  and  Paul, 1980b,  Gilbert  and  Ha,  1984).  However,  the  actual 
implementation  of  such  controller  has  not  been  reported  until  very  recently.  The 
only  experimental  result  that  has  been  published  is  by  Khosla  and  Kanade  (1986), 
who  at  CMU  also  used  a  direct  drive  arm.  The  feedforward  control  for  manipulators 
was  suggested  by  Liegeois,  Fournier,  and  Aldon  (1980),  and  Asada,  Kanade,  and 
Takeyama  (1983)  reported  some  results  of  feedforward  control  implementation  on 
their  direct  drive  arm. 

In  the  study  of  force  control,  there  has  been  much  effort  since  the  late  1970’s. 
The  report  by  Whitney  (1985)  includes  an  extensive  survey  of  force  control  for  ma¬ 
nipulators.  Some  of  the  studies  have  focused  on  passive  compliance  devices  (Drake 


and  Simunovic,  1977).  For  active  force  control,  there  are  mainly  four  continuous 
feedback  methods  that  have  been  proposed  and  implemented  with  various  degree 
of  success  by  researchers.  They  include: 

•  damping  control  (Whitney,  1977), 

•  stiffness  control  (Salisbury,  1980) , 

•  impedance  control  (Hogan,  1985a,  1985b,  1985c), 

•  hybrid  force/position  control  (Raibert  and  Craig,  1981;  Khatib,  1983;  Khatib 
and  Burdick,  1986). 

The  above  force  control  implementations  generally  had  difficulty  in  dealing  with 
stiff  environments.  Recently,  Whitney  (1985),  Roberts,  Paul,  and  Hillberry  (1985), 
Wlassich  (1986),  and  Eppingerand  Seering  (1986)  have  addressed  stability  problems 
for  force  feedback  algorithms  using  wrist  force  sensors.  In  some  instances,  the  work 
reported  in  this  thesis  is  similar  to  their  work,  but  includes  more  complete  analyses, 
verification  by  actual  experiments,  and  some  novel  approaches  in  remedying  the 
stability  problems. 

There  has  been  much  discussion  on  the  singularities  of  the  Jacobian  inverse  often 
used  in  force  control  algorithms  (Whitney,  1972).  But  there  has  been  no  previous 
work  in  considering  the  instabilities  caused  by  the  coordinate  transformations  at 
places  other  than  the  kinematic  singularity  points.  To  the  best  of  my  knowledge, 
the  results  reported  in  this  thesis  on  the  kinematic  instabilities  of  some  force  control 
methods  are  the  first  in  this  area. 

1.4  Overview  of  the  Thesis 

In  Chapter  2,1  the  estimation  algorithm  for  a  load  is  presented  along  with  exper¬ 
imental  results  on  the  PUMA  and  the  DDARM.  The  algorithm  is  based  on  the 
reformulation  of  the  Newton-Euler  rigid  body  dynamics  of  a  load  such  that  the 
resulting  equations  are  linear  in  terms  of  the  unknown  inertial  parameters.  The 
load  identification  algorithm  is  extended  in  Chapter  3  to  identify  all  of  the  inertial 


parameters  of  the  links  of  any  manipulator  whose  torque  or  force  at  each  joint  can 
be  measured.  The  algorithm  is  implemented  successfully  to  identify  the  link  inertial 
parameters  of  the  DDARM.  The  estimated  parameters  are  used  in  the  feedforward 
and  the  computed  torque  control  algorithms  and  their  performances  are  compared 
in  Chapter  4  against  the  performance  of  a  simple  PD  controller. 

Chapters  5  and  6  deal  with  the  stability  problems  of  force  control.  The  dynamic 
instabilities,  observed  in  a  force  controlled  manipulator  in  contact  with  a  stiff  en¬ 
vironment,  are  studied  in  Chapter  5.  Analytical  results  using  a  simple  model  of 
a  manipulator  are  presented  and  verified  by  experiments.  Then,  some  methods  of 
solving  the  instability  problems  are  suggested  and  demonstrated  on  the  third  link  of 
the  DDARM.  In  Chapter  6,  another  type  of  instability,  associated  with  some  force 
control  methods,  is  studied.  Three  different  force  control  methods  are  considered 
and  it  is  shown  both  by  analyses  and  by  experiments  that  the  hybrid  control  method 
of  Raibert  and  Craig  (1981)  exhibit  kinematically  induced  instabilities  whereas  the 
other  methods  do  not. 

In  Chapter  7,  the  results  of  the  estimation,  the  trajectory  control,  and  the 
dynamic  and  kinematic  instability  analyses  are  combined  in  implementing  a  stable 
force/position  controller  on  the  two  joints  of  the  DDARM.  Finally,  conclusions  and 
recommendation  for  future  work  are  presented  in  Chapter  8. 


Chapter  2 


Estimation  of  Load  Inertial 
Parameters 


2.1  Introduction 

This  chapter  presents  a  method  of  estimating  all  of  the  inertial  parameters  of  a  rigid 
body  load  using  a  wrist  force/torque  sensor:  the  mass,  the  moments  of  inertia,  the 
location  of  its  center  of  mass,  and  the  object’s  orientation  relative  to  a  force  sensing 
coordinate  system.  This  procedure  has  three  steps: 

1.  A  Newton-Euler  formulation  for  the  rigid  body  load  yields  dynamics  equations 
linear  in  the  unknown  inertial  parameters,  when  the  moment  of  inertia  tensor 
is  expressed  about  the  wrist  force  sensing  coordinate  system  origin. 

2.  These  inertial  parameters  are  then  estimated  using  a  least 
squares  estimation  algorithm. 

3.  The  location  of  the  load’s  center  of  mass,  its  orientation,  and  its  principal 
moments  of  inertia  can  be  recovered  from  the  sensor  referenced  estimated 
parameters. 

In  principle,  there  are  no  restrictions  on  the  movements  used  to  do  this  load  iden¬ 
tification,  except  that  if  accurate  estimation  of  all  the  parameters  is  desired  the 

This  chapter  is  a  revised  version  of  (Atkeson,  An,  and  Hollerbach,  1985b) 
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motion  must  be  sufficiently  rich  (i.e.  occupy  more  than  one  orientation  with  re¬ 
spect  to  gravity  and  contain  angular  accelerations  in  several  different  directions) 
and  sometimes  special  test  movements  must  be  used  to  get  accurate  estimates  of 
moment  of  inertia  parameters. 

There  are  several  applications  for  this  load  identification  procedure.  The  accu¬ 
racy  of  path  following  and  general  control  quality  of  manipulators  moving  external 
loads  can  be  improved  by  incorporating  a  model  of  the  load  into  the  controller, 
as  the  effective  inertial  parameters  of  the  last  link  of  the  manipulator  change  with 
the  load.  The  mass,  the  center  of  mass,  and  the  moments  of  inertia  constitute  a 
complete  set  of  inertial  parameters  for  an  object;  in  most  cases,  these  parameters 
form  a  good  description  of  the  object,  although  they  do  not  uniquely  define  it.  The 
object  may  be  completely  unknown  at  first  and  an  inertial  description  of  the  object 
may  be  generated  as  the  robot  picks  up  and  moves  the  object.  The  robot  may  also 
be  used  in  a  verification  process,  in  which  the  desired  specification  of  the  object  is 
known  and  the  manipulator  examines  the  object  to  verify  if  it  is  within  the  toler¬ 
ances.  Recognition,  finding  the  best  match  of  a  manipulated  object  to  one  among 
a  set  of  known  objects,  may  also  be  desired.  Finally,  the  estimated  location  of  the 
center  of  mass  and  the  orientation  of  the  principal  axis  can  be  used  to  verify  that 
the  manipulator  has  grasped  the  object  in  the  desired  manner. 

A  key  feature  of  the  approach  in  this  thesis  is  that  it  requires  no  special  test 
or  identification  movements  and  therefore  can  continuously  interpret  wrist  force 
and  torque  sensory  data  during  any  desired  manipulation.  Previous  methods  of 
load  identification  were  restricted  in  their  application.  Paul  (1981)  described  two 
methods  of  determining  the  mass  of  a  load  when  the  manipulator  is  at  rest,  one 
requiring  the  knowledge  of  joint  torques  and  the  other  forces  and  torques  at  the 
wrist.  The  center  of  mass  and  the  load  moments  of  inertia  were  not  identified. 

Coiffet  (1983)  utilized  joint  torque  sensing  to  estimate  the  mass  and  center  of 
mass  of  a  load  for  a  robot  at  rest.  Moments  of  inertia  were  estimated  with  special 
test  motions,  moving  only  one  axis  at  a  time  or  applying  test  torques.  Because  of 
the  intervening  link  masses  and  domination  of  inertia  by  the  mass  moments,  joint 
torque  sensing  is  less  accurate  than  wrist  force-torque  sensing. 

Olsen  and  Bekey  (1985)  assumed  full  force-torque  sensing  at  the  wrist  to  identify 
the  load  without  special  test  motions.  Mukerjee’s  approach  (1984,  1985),  which  also 
allows  general  motion  during  load  identification,  is  similar  to  the  approach  taken  in 


this  chapter.  Nevertheless,  neither  paper  simulated  or  experimentally  implemented 
their  procedures  to  verify  the  correctness  of  the  equations  or  to  determine  the 
accuracy  of  estimation  in  the  presence  of  noise  and  imperfect  measurements. 

Khalil,  Gautier,  and  Kleinfinger  (1986)  have  suggested  a  method  of  identify¬ 
ing  the  load  inertial  parameters  using  joint  torque  sensing  as  a  part  of  their  link 
identification  procedure.  As  mentioned  before,  joint  torque  sensing  is  less  accu¬ 
rate  than  wrist  force-torque  sensing.  Also,  they  did  not  present  any  simulation  or 
experimental  results. 

The  algorithm  to  be  presented  requires  measurements  of  the  force  and  torque 
due  to  a  load  and  measurements  or  estimates  of  the  position,  velocity,  acceleration, 
orientation,  angular  velocity,  and  angular  acceleration  of  the  force  sensing  coordi¬ 
nate  system.  It  can  handle  incomplete  force  and  torque  measurement  by  simply 
eliminating  the  equations  containing  missing  measurements.  The  necessary  kine¬ 
matic  data  can  be  obtained  from  the  joint  angles  and,  if  available,  the  joint  velocities 
of  the  manipulator.  Also,  the  inertial  parameters  of  a  robot  hand  can  be  identified 
using  this  algorithm  and  then  the  predicted  forces  and  torques  due  to  the  hand  can 
be  subtracted  from  the  sensed  forces  and  torques,  so  that  only  the  load  is  estimated. 

This  inertial  parameter  estimation  algorithm  was  implemented  using  a  PUMA 
600  robot  equipped  with  an  RTI  FS-B  wrist  force/ torque  sensor,  and  on  the  MIT 
Serial  Link  Direct  Drive  Arm  (DDARM)  equipped  with  a  Barry  Wright  Company 
Astek  FS6-120A-200  6-axis  wrist  force/torque  sensor. 

2.2  The  Newton-Euler  Approach  To  The  Load 
Identification  Problem 

2.2.1  Deriving  The  Parameter  Equation 

To  derive  equations  for  estimating  the  unknown  inertial  parameters,  the  co¬ 
ordinate  systems  in  Figure  2.1  are  used  to  relate  different  coordinate  frames  and 
vectors.  O  is  an  inertial  or  base  coordinate  system,  which  is  fixed  in  space  with 
gravity  pointing  along  the  —  z  axis.  P  is  the  force  reference  coordinate  system  of  a 
wrist  force/torque  sensor  rigidly  attached  to  the  load.  Q  represents  the  principal 
axis  of  the  rigid  body  load  located  at  the  center  of  mass.  The  x  axis  of  Q  is  along 
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the  largest  principal  moment  of  inertia,  and  the  z  axis  along  the  smallest.  Q  is 
unique  up  to  a  reflection  in  bodies  with  3  distinct  principal  moments  of  inertia.  In 
the  derivation  that  follows  all  vectors  are  initially  expressed  in  the  base  coordinate 
system  O. 

The  mass,  moments  of  inertia,  location  of  the  center  of  mass,  and  orientation  of 
the  body  (a  rotation  gPR  from  the  principal  axes  to  the  force  reference  system)  are 
related  to  the  motion  of  the  load  and  the  forces  and  torques  exerted  on  it  by  the 
Newton-Euler  equations.  The  net  force  ,f  and  the  net  torque  9n  acting  on  the  load 
at  the  center  of  mass  are: 

,f  =  f  +  mg  =  mq  (2.1) 

4n  =  n-  cxf  =  ,I<j  +  uf  x  (,1a;)  (2.2) 

where: 


f  =  the  force  exerted  by  the  wrist  sensor  on  the  load  at  the 
point  p, 

m  =  the  mass  of  the  load, 

g  =  the  gravity  vector  (g  =  [0,0,  —9.8  meters/sec2]), 
q  =  the  acceleration  of  the  center  of  mass  of  the  load, 
n  =  the  torque  exerted  by  the  wrist  sensor  on  the  load  at  the 
point  p, 

c  =  the  unknown  location  of  the  center  of  mass  relative  to  the 
force  sensing  wrist  origin  P, 

9I  =  the  moment  of  inertia  tensor  about  the  center  of  mass, 
u  =  the  angular  velocity  vector,  and 
(If  =  the  angular  acceleration  vector. 
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To  formulate  an  estimation  algorithm,  the  force  and  torque  measured  by  the 
wrist  sensor  must  be  expressed  in  terms  of  the  product  of  known  geometric  param¬ 
eters  and  the  unknown  inertial  parameters.  Although  the  location  of  the  center  of 
mass  and  hence  its  acceleration  q  are  unknown,  q  is  related  to  the  the  acceleration 
of  the  force  reference  frame  p  by 

q  =  p  +  iixc+ux  (u/  x  c)  (2.3  [7.40]1) 

Substituting  (2.3)  into  (2.1), 

f  =  mp  —  mg  +  (1/  x  me  +  u  x  (u  x  me)  (2.4) 

Substituting  (2.4)  into  (2.2), 

n  =  ,1(1/  +  ux  (,Iu/)  4-  me  x  (w  x  c)  +  me  x  (a/  x  (t«/  x  c)) 

+  me  x  p  —  me  x  g  (2.5) 

Although  the  terms  c  x  (w  x  c)  and  c  x  (a/  X  (u/  x  c))  are  quadratic  in  the 
unknown  location  of  the  center  of  mass  c,  these  quadratic  terms  are  eliminated  by 
expressing  the  moment  of  inertia  tensor  about  the  force  sensor  coordinate  origin 
(PI)  instead  of  about  the  center  of  mass  (,I).  Rewriting  (2.5)  as: 

n  =  ,1(1/  +  (*/  x  (,Iw)  +  m[(c7'c)l  -  (ccr)jd/ 

+  u/  x  (m[(cTc)l  —  (ccT)]w)  -f  me  x  p  —  me  x  g  (2.6) 

and  using  the  three  dimensional  version  of  the  parallel  axis  theorem 

PI  =  ,1  +  m[(cTc)l  -  (ccT)]  (2.7  [10.147]) 

to  simplify  it  results  in: 

n  =  plw  +  a/  x  (pIw)  +  me  x  p  —  me  x  g  (2.8) 

(1  is  the  3  dimensional  identity  matrix).  All  the  vectors  are  expressed  in  the  wrist 
sensor  coordinate  system  P,  so  that  the  quantities  c  and  pI  are  constant.  More¬ 
over,  the  wrist  force/torque  sensor  measures  forces  and  torques  directly  in  the  P 
coordinate  frame. 

'Equation  numbers  in  brackets  refer  to  equations  in  Symon,  1071. 
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In  order  to  formulate  the  above  equations  as  a  system  of  linear  equations,  the 
following  equalities  and  notations  are  used: 


u  x  c  = 


0 

—  CJ; 

uM 

0 

-Wy 

w. 

0  — t J, 

o.  0 


Cy  =  [wxj  C 

C« 


u,  uv  u,  0  0  0 

Iw  =  0  u>x  0  uv  uM  0 

0  0  <js  0  a/y  a/. 


A  r 

=  [#U> 


(2.10) 


where 


7n  hi  hs 


I  =  IT  =  /12  722  hs  (2-H) 

7lS  723  Tjs 

Using  these  expressions,  Eqs.  (2.4)  and  (2.8)  can  be  written  as  a  single  matrix 
equation  in  the  wrist  sensor  coordinate  frame: 


p-g  [«x]  +  [wx][wx]  0 

o  ((g-p)xj  H  +  [wx]M 


(2.12) 


or  more  compactly, 

w  =  A<f>  (2.13) 

where  w  is  a  6  element  wrench  vector  combining  both  the  force  and  torque  vectors, 
A  is  a  6  x  10  matrix  and  ^  is  the  vector  of  the  10  unknown  inertial  parameters. 
Note  that  the  center  of  mass  cannot  be  determined  directly,  but  only  as  the  mass 


moment  me.  But  since  the  mass  m  is  separately  determined,  its  contribution  can 
be  factored  from  the  mass  moment  later. 


2.2.2  Estimating  The  Parameters 


The  quantities  inside  the  A  matrix  are  computed  by  direct  kinematics  computation 
(Luh,  Walker,  and  Paul,  1980a)  from  the  measured  joint  angles.  The  elements  of 
the  w  vector  are  measured  directly  by  the  wrist  force  sensor.  Since  (2.13)  represents 
6  equations  and  10  unknowns,  at  least  two  data  points  are  necessary  to  solve  for  the 
^  vector,  i.e.  the  force  and  the  position  data  sampled  at  two  different  configurations 
of  the  manipulator.  For  robust  estimates  in  the  presence  of  noise,  a  larger  number 
of  data  points  must  be  used.  Each  data  point  adds  6  more  equations,  whereas  the 
number  of  unknowns,  the  elements  of  remain  constant.  This  can  be  represented 
by  augmenting  w  and  A  as: 


A  = 

'  AM  ' 

,  w  = 

w[l] 

A\n ] 

w  [n] 

n  =  number  of  data  points  (2.14) 


where  each  A[»]  and  w[t]  are  matrix  and  vector  quantities  described  in  (2.12).  For¬ 
mulated  this  way,  any  linear  estimation  algorithm  can  be  used  to  identify  the  <f> 
vector.  A  simple  and  popular  method  is  the  least  squares  method.  The  estimate 
for  <f>  is  given  by: 

4  =  (ArA)-IArw  (2.15) 

Equation  (2.15)  can  also  be  formulated  in  a  recursive  form  (Ljung  and  Soderstrom, 
1983)  for  on-line  estimation. 


2.2.3  Recovering  Object  And  Grip  Parameters 

The  estimated  inertial  parameters  (m,  me,  PI)  are  adequate  for  control,  but  for  ob¬ 
ject  recognition  and  verification  it  is  also  necessary  to  obtain  the  principal  moments 
of  inertia  7j,  J2,  J3,  the  location  of  the  center  of  mass  c,  and  the  orientation  QPR  of 
Q  with  respect  to  P. 
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The  parallel  axis  theorem  is  used  to  compute  the  inertia  terms  translated  to  the 
center  of  mass  of  the  load. 

c  =  —  (2.16) 

(2.17) 


me 

m 


qI  =  pi  -  m[(cTc)l  -  (ccT)] 

Then,  the  principal  moments  are  obtained  by  diagonalizing  4I. 


A  0  0 
0  A  0 
0  0  A 


(2.18) 


This  diagonalization  can  always  be  achieved  since  ,1  is  symmetric;  but  when  two  or 
more  principal  moments  are  equal,  the  rotation  matrix  QPR  is  no  longer  unique. 


2.3  Experimental  Results 

2.3.1  Estimation  on  the  PUMA  Robot 

The  inertial  parameter  estimation  algorithm  was  originally  implemented  on  a  PUMA 
600  robot  equipped  with  an  RTI  FS-B  wrist  force/ torque  sensor  (Figure  2.2),  which 
measures  all  six  forces  and  torques.  The  PUMA  600  has  encoders  at  each  joint 
to  measure  joint  angles,  but  no  tachometers.  Thus,  to  obtain  the  joint  velocities 
and  accelerations,  the  joint  angles  are  differentiated  and  double-differentiated,  re¬ 
spectively,  by  a  digital  differentiating  filter  (Figure  2.3).  The  cutoff  frequency  of 
33  Hz  for  the  filter  was  determined  empirically  to  produce  the  best  results.  Both 
the  encoder  data  the  wrist  sensor  data  were  initially  sampled  at  1000  Hz.  It  was 
later  determined  that  a  sampling  rate  of  200  Hz  was  sufficient,  and  the  data  were 
resampled  at  the  lower  rate  to  reduce  processing  time.  A  least  squares  identification 
algorithm  was  implemented  as  an  off-line  computation,  but  an  on-line  implementa¬ 
tion  would  have  been  straightforward. 


Static  Estimation  Using  The  PUMA 

To  test  the  calibration  of  the  force  sensor  and  the  kinematics  of  the  PUMA  arm  a 
static  identification  was  performed.  The  forces  and  torques  are  now  due  only  to  the 
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Parameters 

Actual 

Values 

Static 

Estimates 

Dynamic 

Estimates 

Mass  (kg) 

Change  in  c„(m) 

1.106 

0.037 

1.103 

0.037 

1.067 

0.039 

Mass  (kg) 

Change  in  ey(m) 

1.106 

-0.043 

1.107 

-0.043 

1.084 

-0.042 

Mass  (kg) 

Change  in  ev(m) 

1.106 

-0.021 

1.100 

-0.020 

1.073 

-0.021 

Mass  (kg) 

Change  in  e„(m) 

1.106 

0.018 

1.099 

0.018 

1.074 

0.020 

Table  2.1:  Mass  and  the  Center  of  Mass  Estimates 
gravity  acting  on  the  load,  and  equations  (2.4)  and  (2.8)  simplify  to 

f  =  -mg  (2.19) 

n  =  -me  x  g  (2.20) 

As  seen  in  (2.19)  and  (2.20),  only  the  mass  and  the  center  of  mass  can  be  identified 
while  the  manipulator  is  stationary. 

To  avoid  needing  to  determine  the  gripper  geometric  parameters,  the  center 
of  mass  estimates  are  evaluated  by  the  estimates  of  the  changes  in  the  center  of 
mass  as  the  load  is  moved  along  the  y-axis  from  the  reference  position  by  known 
amounts.  The  results  of  estimation  are  shown  in  the  third  column  of  Table  2.1  for 
an  aluminum  block  (2  x  2  x  6  m.)  with  a  mass  of  1.106  K g.  Only  the  changes  in 
cy  are  shown  in  Table  2.1;  the  estimates  of  cx  and  cM  remained  within  1mm  of  the 
reference  values  (c*  =  1  mm  and  c,  =  47  mm).  Each  set  of  estimates  were  computed 
from  6  sets  of  data,  i.e.  data  taken  at  6  different  positions  and  orientations  of  the 
manipulator,  where  each  data  point  is  averaged  over  1000  samples  to  minimize  the 
effects  of  noise.  The  results  show  that  in  the  static  case  the  mass  of  the  load  can  be 
estimated  to  within  10<7  of  the  actual  mass.  The  center  of  mass  can  be  estimated 
to  within  1mm  of  the  actual  values  for  this  load. 

Static  load  estimation  only  tests  the  force  sensor  calibration  and  the  position 
measurement  capabilities  of  the  robot  on  which  the  sensor  is  mounted.  In  order  to 


assess  the  effects  of  the  dynamic  capabilities  of  the  robot  on  load  estimation  and 
to  be  able  to  estimate  the  moments  of  inertia  of  the  load,  it  is  necessary  to  assess 
parameter  estimation  during  general  movement. 

Dynamic  Estimation  Using  The  PUMA 

In  the  dynamic  case,  the  joint  position  encoder  and  the  wrist  sensor  data  are  sam¬ 
pled  while  the  manipulator  is  in  motion.  A  fifth  order  polynomial  trajectory  in  joint 
space  was  used  to  minimize  the  mechanical  vibrations  at  the  beginning  and  the  end 
of  the  movement,  and  to  improve  the  signal  to  noise  ratio  (SNR)  in  the  acceler¬ 
ation  data  (Figure  2.3).  For  more  popular  bang-coast-bang  type  trajectories,  the 
joint  accelerations  are  zero  except  at  the  beginning  and  the  end  of  the  movements, 
resulting  in  poor  SNR  in  the  acceleration  data  for  most  of  the  movement. 

The  PUMA  robot  lacked  the  acceleration  capacity  necessary  to  estimate  the  mo¬ 
ments  of  inertia  of  the  load.  It  also  lacked  true  velocity  sensors  at  the  joints,  which 
made  estimation  of  the  acceleration  of  the  load  difficult.  The  dynamic  estimates 
of  mass  and  center  of  mass  for  the  previous  load  are  shown  in  the  last  column  of 
Table  2.1.  The  data  used  in  these  estimates  were  sampled  while  the  manipulator 
was  moving  from  [0,0,0, —90,0,0]  to  [90,-60,90,90,90,90]  degrees  on  a  straight 
line  in  joint  space  in  2  seconds.  Joint  4  of  the  PUMA  has  a  higher  maximum  ac¬ 
celeration  than  the  other  joints,  and  thus,  a  longer  path  was  given  for  it.  This 
movement  was  the  fastest  the  PUMA  can  execute  using  the  fifth  order  trajectory 
without  reaching  the  maximum  acceleration  for  any  of  its  joints.  The  estimates 
used  all  400  data  points  sampled  during  the  2  second  movement.  The  results  show 
slight  deterioration  in  these  estimates  when  compared  to  the  static  estimates;  but 
they  are  still  within  40 g  and  2mm  of  the  actual  mass  and  displacement,  respectively. 
However,  the  SNR  in  the  acceleration  and  the  force/torque  data  were  too  low  for 
accurate  estimates  of  the  moments  of  inertia  for  this  load  (0.00238X5  •  m2  in  the 
largest  principal  moment).  In  this  case,  the  torque  due  to  gravity  is  approximately 
40  times  greater  than  the  torque  due  to  the  maximum  angular  acceleration  of  the 
load.  Thus,  even  slight  noise  in  the  data  would  result  in  poor  estimates  of  I. 
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Parameters 
(kg  •  m3) 

Actual 

V  alues 

PUMA1 

Estimates 

PUMA3 

Estimates 

DDARM1 

Estimates 

hi 

0.0244 

0.0192 

0.0246 

0.0233 

As 

0 

-0.0048 

0.0006 

0.0003 

As 

0 

0.0019 

0.0008 

0.0007 

As 

0.0007 

0.0021 

0.0036 

0.0001 

As 

0 

-0.0016 

-0.0004 

-0.0002 

As 

0.0242 

0.0176 

0.0199 

0.0236 

1  (all  joints  moving) 

3  (3  special  test  movements  combined) 


Table  2.2:  Estimates  of  the  moments  of  inertia 

Special  Test  Movements  Using  The  PUMA 

Therefore,  experiments  with  a  different  load  were  performed  for  the  estimates  of  the 
moments  of  inertia.  The  new  experimental  load  is  shown  in  Figure  2.2.  This  load 
has  large  masses  at  the  two  ends  of  the  aluminum  bar,  resulting  in  large  moments 
of  inertia  in  two  directions  (~  0.024 kg  •  m3)  and  a  small  moment  in  the  other.  A 
typical  set  of  estimates  of  the  moments  of  inertia  at  the  center  of  mass  frame  for 
the  load  with  the  gripper  subtracted  out  are  shown  in  Table  2.3.1  for  the  above 
all-joints-moving  trajectory.  They  contain  some  errors  but  are  fairly  close  to  the 
actual  values. 

In  order  to  improve  the  estimates,  the  data  were  sampled  while  the  robot  was 
following  three  separate  2-second  rotational  trajectories  around  the  principal  axes 
of  the  load.  Such  trajectories  used  joint  4  and  joint  6  only,  and  resulted  in  higher 
acceleration  data  than  the  previous  trajectory,  thus  improving  the  SNR  in  both  the 
acceleration  and  the  force/torque  data.  Typical  estimates  for  these  special  move¬ 
ments  show  improvements  over  the  estimates  with  the  previous  trajectory  (Table 
2.3.1).  Although  the  estimate  of  I22  is  slightly  worse  than  before,  all  the  other  terms 
have  improved;  the  cross  terms,  especially,  are  much  smaller  than  before.  However, 
these  estimates  of  I  are  not  as  accurate  as  the  estimates  of  the  mass  and  the  center 
of  mass  shown  in  Table  2.1.  Most  of  the  error  is  probably  due  to  the  large  amount 
of  noise  present  in  the  acceleration  data  caused  by  differentiating  the  joint  angle 
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data  twice.  Part  of  the  error  may  be  due  to  inaccuracies  in  the  current  kinematic 
parameters  of  the  manipulator.  Experiments  have  shown  that  the  actual  orientation 
of  the  robot  can  be  up  to  4°  off  from  the  orientation  computed  from  the  encoder 
data. 

Figure  2.4  shows  the  comparison  of  the  measured  forces  and  torques,  and  the 
computed  forces  and  torques  generated  by  a  simulator  from  the  estimated  parame¬ 
ters  and  the  measured  joint  data.  The  two  sets  of  figures  match  very  well  even  in  the 
mechanical  vibrations,  verifying  qualitatively  the  accuracy  of  the  estimates.  This 
suggests  that  for  control  purposes  even  poor  estimation  of  moment  of  inertia  pa¬ 
rameters  will  allow  good  estimates  of  the  total  force  and  torque  necessary  to  achieve 
a  trajectory.  This  makes  good  sense  in  that  the  load  forces  with  the  PUMA  are 
dominated  by  gravitational  componenents,  and  angular  accelerations  experienced 
by  the  load  are  small  relative  to  those  components. 

2.3.2  The  MIT  Serial  Link  Direct  Drive  Arm 

The  effect  of  the  errors  causing  poor  estimates  of  moment  of  inertia  parameters 
could  be  alleviated  by  increasing  the  angular  acceleration  experienced  by  the  load. 
Since  the  PUMA  robot  was  already  operating  at  its  limits  for  the  experimental 
results  presented  above,  the  algorithm  was  next  implemented  on  the  MIT  Serial 
Link  Direct  Drive  Arm,  which  has  much  higher  acceleration  and  velocity  capability. 
The  DDARM  also  has  a  tachometer  on  each  of  its  three  joints  so  that  numerical 
differentiation  of  positions  is  unnecessary;  but  the  accelerations  were  obtained  by 
digitally  differentiating  the  velocities  using  a  cutoff  frequency  of  30Hz.  A  Barry 
Wright  Company  Astek  FS6-120A-200  6-axis  force/torque  sensor  was  used  to  ;nea- 
sure  all  three  forces  and  three  torques  about  a  point.  The  positions  and  velocities 
of  the  robot  were  initially  sampled  at  1kHz  but  were  later  down-sampled  to  match 
the  sampling  frequency  of  the  force/torque  sensor  of  240  Hz.  The  identification 
procedure  was  again  implemented  off-line. 

Dynamic  Estimation  Using  The  Direct  Drive  Arm 

The  data  used  for  estimating  the  inertial  parameters  of  the  load  were  sampled  while 
the  manipulator  was  moving  from  (280,269.1,-30)  to  (80, 19.1,220)  in  one  second. 
Again  a  fifth  order  polynomial  straight  line  trajectory  in  joint  space  was  used.  The 
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Figure  2.5:  Measured  force/torque  data  and  computed  force/torque  data 
estimates  using  the  Direct  Drive  Arm. 
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resulting  estimates  for  the  moment  of  inertia  parameters  are  shown  in  the  last 
column  of  Table  2.3.1.  The  estimates  for  the  mass  and  the  location  of  the  center  of 
mass  were  as  good  as  the  PUMA  results  and  are  not  shown.  The  estimated  moment 
of  inertia  parameters  are  on  the  whole  better  than  the  PUMA  results. 

Parameters  estimated  for  a  set  of  special  test  movements  using  the  direct  drive 
arm  were  not  substantially  different.  The  special  test  movements  for  the  DDARM 
were  not  substantially  faster  than  the  movement  of  all  joints,  and  thus  probably 
contained  the  same  amount  of  information. 

Finally,  Figure  2.5  shows  the  comparison  of  typical  measured  forces  and  torques 
with  computed  forces  and  torques  generated  by  a  simulator  from  the  estimated 
parameters  and  the  measured  joint  data.  Once  again  there  is  a  very  good  match 
between  the  measured  and  the  predicted  forces  and  torques.  Thus,  as  expected, 
the  combination  of  higher  angular  accelerations  and  good  velocity  sensing  results 
in  better  parameter  estimates, 

2.4  Discussion 

2.4.1  Usefulness  of  the  Algorithm 

It  is  important  to  realize  that  there  are  two  distinct  uses  of  an  identified  model. 
For  control  what  matters  is  matching  the  input-output  behavior  of  the  model  (in 
this  case  the  relationship  of  load  trajectory  to  load  forces  and  torques)  to  reality, 
while  for  recognition/ verification  what  matters  is  matching  estimated  parameters 
to  a  set  of  parameters  postulated  for  reality.  Both  implementations  of  load  inertial 
parameter  estimation  successfully  match  the  input-output  behavior  of  the  load  ( 
Figures  2.4  and  2.5),  However,  the  limited  acceleration  capacity  of  the  PUMA  robot 
and  its  limited  sensing  result  in  relatively  poor  estimates  of  the  moments  of  inertia 
of  the  load  without  the  use  of  special  test  motions.  In  all  cases  the  mass  and  the 
location  of  the  center  of  mass  could  be  accurately  estimated  from  both  series  of 
static  measurements,  and  dynamic  measurements.  Hence,  identifying  parameters 
well  enough  for  recognition  of  the  object  may  require  large  accelerations  or  special 
test  movements  in  order  to  obtain  the  moment  of  inertia  parameters  accurately. 
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2.4.2  Sources  of  Error 


This  work  is  preliminary  in  that  an  adequate  statistical  characterization  of  the 
errors  of  the  estimated  parameters  of  the  predicted  forces  has  not  been  attempted. 
Nevertheless,  some  insights  were  gained  into  the  sources  of  such  errors. 

The  ultimate  source  of  error  is  the  random  noise  inherent  in  the  sensing  process 
itself.  The  noise  levels  on  the  position  and  velocity  sensing  are  probably  negligible, 
and  could  be  further  reduced  by  appropriate  filtering  using  a  model  based  filter 
such  as  the  Extended  Kalman  Filter.  The  force  and  torque  measurement  process 
involve  measuring  strain  of  structure  members  in  the  sensor  with  semiconductor 
strain  gages.  The  random  noise  involved  in  such  measurements  is  also  probably 
negligible. 

Bias  Errors 

However,  semiconductor  strain  gages  are  notoriously  prone  to  drift.  Periodic  recal¬ 
ibration  of  the  offsets  (very  often)  and  the  strain-to-force  calibration  matrix  (often) 
may  be  necessary  to  reduce  load  parameter  estimation  errors  further.  During  the 
experiments  presented  in  this  chapter,  in  order  to  minimize  the  bias  errors,  the  data 
were  taken  after  the  force  sensors  had  been  left  on  for  a  while  to  warm  up  and  the 
offsets  were  recalibrated  before  each  data  collection  session. 

Unmodelled  Dynamics 

A  further  source  of  noise  is  unmodelled  structural  dynamics.  Neither  the  robot 
links  nor  the  load  itself  are  perfectly  rigid  bodies.  A  greater  source  of  concern  is  the 
compliance  of  the  force  sensor  itself.  In  order  to  generate  structural  strains  large 
enough  to  be  reliably  measured  with  even  semiconductor  strain  gage  technology, 
a  good  deal  of  compliance  is  introduced  into  the  force  sensor.  The  load  rigidly 
attached  to  the  force  sensor  becomes  a  relatively  undamped  spring  mass  system. 
The  response  of  the  Astek  force  sensor  to  a  tap  on  an  attached  load  is  shown  in  the 
“undamped  impulse  response”  record  of  Figure  2.6.  The  effect  of  robot  movement 
on  this  spring  mass  system  is  shown  in  the  “undamped  movement  response”  record. 

There  are  several  approaches  to  ta  e  to  deal  with  this  problem  of  unmodelled  dy¬ 
namics.  One  approach  is  to  attempt  to  identify  the  additional  dynamics.  However, 


this  greatly  increases  the  complexity  of  the  identification  process  and  the  amount 
of  data  that  needs  to  be  collected  to  get  reliable  estimates  of  any  parameter. 

Another  approach  is  to  try  to  avoid  exciting  the  unmodelled  dynamics  by  choos¬ 
ing  robot  trajectories  that  were  as  smooth  as  possible.  Therefore,  the  5th  order 
polynomial  trajectories  were  chosen  in  the  experiments  so  that  the  velocities  and 
accelerations  are  always  continuous.  Using  higher  order  polynomials  would  result 
in  even  greater  smoothness.  However,  with  the  PUMA  a  smooth  commanded  tra¬ 
jectory  did  not  result  in  a  smooth  actual  trajectory,  because  the  control  methods 
used  and  the  actual  hardware  of  the  robot  still  introduced  substantial  vibration. 
One  way  to  tell  if  the  PUMA  is  turned  on  is  to  touch  it  and  feel  if  it  is  vibrating. 
Vibrations  were  less  of  a  problem  with  the  direct  drive  arm,  although  still  present. 

The  most  successful  approach  is  to  mechanically  damp  out  the  vibrations  by 
introducing  some  form  of  energy  dissipation  into  the  structure.  Hard  rubber  washers 
were  added  between  the  force  sensor  and  the  load.  The  “damped  impulse  response” 
of  Figure  2.6  illustrates  the  response  of  the  force  sensor  to  a  tap  on  the  load.  The 
oscillations  decay  much  faster  with  the  added  damping.  The  “damped  movement 
response”  indicates  that  this  mechanical  damping  also  greatly  reduces  the  effect  of 
movement  on  the  resonant  modes  of  the  force  sensor  plus  load. 

A  better  method  would  have  been  to  design  appropriate  damping  into  the  force 
sensors,  just  as  accelerometers  are  filled  with  oil  to  provide  a  critically  damped 
response  for  a  specified  measurement  bandwidth.  Failing  that,  energy  dissipation 
must  be  introduced  either  into  the  structural  components  of  the  robot  or  into  the 
gripper  either  structurally  or  as  a  viscous  skin.  As  will  be  discussed  later,  appropri¬ 
ate  mechanical  damping  may  also  be  useful  when  using  a  force  sensor  in  closed-loop 
force  control. 

Optimal  Filtering 

Finally,  the  need  to  numerically  differentiate  the  velocity  to  find  the  acceleration 
greatly  amplifies  whatever  noise  is  present.  One  can  avoid  the  need  to  explicitly 
calculate  acceleration  by  integrating  equations  (2.4)  and  (2.8).  However,  since  an 
integrator  is  an  infinite  gain  filter  at  the  0  frequency,  large  errors  can  result  from 
small  low  frequency  errors  such  as  offsets.  The  best  performance  will  be  achieved 
from  applying  some  “optimal”  filter,  whose  shape  is  probably  an  integrator  at  high 


frequencies  but  a  differentiator  at  lower  frequencies  (Atkeson,  1986). 


2.4.3  Inaccurate  Estimates  of  the  Moments  of  Inertia 

One  of  the  factors  that  make  it  difficult  to  identify  moments  of  inertia  accurately  is 
the  typically  large  contribution  of  gravitational  torque,  which  depends  only  on  the 
mass  and  the  relative  location  of  the  center  of  mass  to  the  force  sensing  coordinate 
origin.  A  point  mass  rotated  at  a  radius  of  5cm  from  a  horizontal  axis  must  complete 
a  full  360°  rotation  in  425  milliseconds  for  the  torque  due  to  angular  acceleration 
to  be  equal  to  the  gravitational  torque,  if  a  5th  order  polynomial  trajectory  is  used. 
A  way  to  avoid  gravitational  torques  is  to  rotate  the  load  about  the  vertical  axis, 
or  to  have  the  point  of  force/torque  sensing  close  to  the  center  of  mass. 

A  simple  example  will  illustrate  the  difficulty  of  recovering  principal  moments 
of  inertia,  given  the  moment  of  inertia  tensor  about  the  force  sensing  origin.  The 
principal  moment  of  inertia  of  a  uniform  sphere  surface  is  only  2/7  of  the  total 
moment  of  inertia  when  it  is  rotated  about  an  axis  tangent  to  its  surface,  so  that 
the  effects  of  any  errors  in  estimating  the  mass,  the  location  of  the  center  of  mass, 
and  the  grip  moments  of  inertia  are  amplified  when  the  principal  moment  of  inertia 
is  calculated.  This  problem  can  be  reduced  by  having  the  point  of  force  sensing  as 
close  to  the  center  of  mass  as  possible 

It  still  may  be  difficult  to  find  the  orientation  of  the  principal  moments  of  inertia 
even  when  the  moment  of  inertia  tensor  about  the  center  of  mass  has  been  estimated 
fairly  accurately.  This  occurs  when  two  or  more  principal  moments  of  inertia  are 
approximately  equal.  Finding  the  orientation  of  the  principal  axis  is  equivalent  to 
diagonalizing  a  symmetric  matrix,  which  becomes  ill-conditioned  when  some  of  the 
eigenvalues  are  almost  equal.  A  two  dimensional  example  illustrates  the  problem. 
Consider  the  diagonalized  matrix 

cos(0)  —  sin(0) 

sin(0)  cos(0) 

with  eigenvalues  {Aj,  A2}  and  whose  first  principal  axis  is  oriented  at  an  angle  0  with 
respect  to  the  x  axis.  When  the  two  eigenvalues  are  almost  equal,  the  terms  of  the 
matrix  dependent  on  the  angle  0  become  very  small.  By  substituting  Ax  —  A2  =  e 


0 

<< 
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cos(0)  sin(0) 
-  sin(0)  cos(0) 


(2.21) 


into  the  matrix  (2.21), 


A2  +  e  cos2(0 )  e  cos(0)  sin(0) 
e  cos(0)  sin(0 )  Aj  -f-  e  sin2  (0) 


(2.22) 


All  terms  that  contain  angle  information  are  multiplied  by  the  difference  (e)  of 
the  principal  moments  of  inertia.  With  a  fixed  amount  of  noise  in  each  of  the  entries 
of  the  identified  moment  of  inertia  matrix,  the  orientation  of  the  principal  axis  (0) 
will  become  more  and  more  difficult  to  recover. 

2.5  Conclusion 

In  summary,  it  was  demonstrated  that  the  inertial  parameters  of  a  manipulator 
load  can  be  estimated  accurately  enough  for  purposes  of  control.  The  estimation 
algorithm  was  derived  from  the  reformulation  of  the  Newton-Euler  equations  for 
the  rigid-body  dynamics  of  a  load  so  that  the  equations  are  linear  in  terms  of  the 
unknown  inertial  parameters.  The  estimation  procedure  then  involved  a  simple 
least  squares  solution  to  a  set  of  linear  equations.  In  Chapter  3,  this  estimation 
method  for  a  load  will  be  extended  to  estimate  the  inertial  parameters  of  all  the 
links  of  a  manipulator. 


Chapter  3 

Estimation  of  Link  Inertial 
Parameters 


3.1  Introduction 

This  chapter  presents  a  method  of  estimating  all  of  the  inertial  parameters,  the 
mass,  the  center  of  mass,  and  the  moments  of  inertia  of  each  rigid  body  link  of  a 
robot  manipulator  using  joint  torque  sensing.  Determining  these  parameters  from 
measurements  or  computer  models  is  generally  difficult  and  involves  some  approxi¬ 
mations  to  handle  the  complex  shapes  of  the  arm  components.  Typically,  even  the 
manufacturers  of  manipulators  do  not  know  accurate  values  of  these  parameters. 

The  degree  of  uncertainty  in  inertial  parameters  is  an  important  factor  in  judg¬ 
ing  the  robustness  of  model-based  control  strategies.  A  common  objection  to  the 
computed  torque  methods,  which  involve  full  dynamics  computation  (e.g.,  Luh, 
Walker,  and  Paul,  1980b),  is  their  sensitivity  to  modelling  errors,  and  a  variety  of 
alternative  robust  controllers  have  been  suggested  (Samson,  1983;  Slotine,  1985; 
Spong,  Thorp,  and  Kleinwaks;  1984,  Gilbert  and  Ha,  1984).  Typically  these  robust 
controllers  express  modelling  errors  as  a  differential  inertia  matrix  and  coriolis  and 
gravity  vectors,  but  in  so  doing,  no  rational  basis  is  provided  for  the  source  of  errors 
or  the  bounds  on  errors.  The  error  matrices  and  vectors  combine  kinematic  and  dy- 
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namic  parameter  errors,  but  kinematic  calibration  is  sufficiently  developed  so  that 
very  little  error  can  be  expected  in  the  kinematic  parameters  (Whitney,  Lozinski, 
and  Rourke  ,  1986). 

One  aim  of  this  work  is  to  place  similar  bounds  on  inertial  parameter  errors  by 
explicitly  identifying  the  inertial  parameters  of  each  link  that  go  into  the  making  of 
the  inertia  matrix  and  coriolis  and  gravity  vectors.  The  load  identification  results 
of  Chapter  2  suggests,  for  example,  that  mass  can  be  accurately  identified  to  within 
1%.  Therefore,  an  assumption  of  50%  error  in  link  mass  in  verifying  a  robust 
control  formulation  (Spong,  Thorp,  and  Kleinwaks,  1984)  is  an  unreasonable  basis 
for  argument.  Slotine  (1985)  suggests  that  errors  of  only  a  few  percent  in  inertial 
parameters  make  his  robust  controller  superior  to  the  computed  torque  method, 
but  it  may  well  be  that  these  parameters  can  be  identified  more  accurately  than  his 
assumptions. 

In  this  thesis,  as  an  alternative  approach,  the  inertial  parameters  will  be  esti¬ 
mated  on  the  basis  of  direct  dynamic  measurements.  The  algorithm  of  Chapter  2 
(Atkeson,  An,  and  Hollerbach,  1985b),  used  to  identify  load  inertial  parameters,  can 
be  modified  to  find  link  inertial  parameters  of  a  robot  arm  made  up  of  rigid  parts. 
The  Newton-Euler  dynamic  equations  are  used  to  express  the  measured  forces  and 
torques  at  each  joint  in  terms  of  the  product  of  the  measured  movements  of  the 
rigid  body  links  and  the  unknown  link  inertial  parameters.  These  equations  are 
linear  in  the  inertial  parameters.  However,  unlike  load  estimation,  the  only  sensing 
is  one  component  of  joint  torque,  inferred  from  motor  current.  Coupled  with  re¬ 
stricted  movement  near  the  base,  it  is,  therefore,  not  possible  to  find  all  the  inertial 
parameters  of  the  proximal  links.  As  will  be  seen,  these  missing  parameters  have 
no  effect  on  the  control  of  the  arm. 

In  this  chapter,  manipulators  with  only  revolute  joints  are  discussed  since  han¬ 
dling  prismatic  joints  requires  only  trivial  modifications  to  the  algorithm.  The 
proposed  algorithm  was  verified  by  implementation  on  the  MIT  Serial  Link  Direct 
Drive  Arm. 


3.1.1  Previous  Work 

Mayeda,  Osuka,  and  Kangawa  (1984)  required  three  sets  of  special  test  motion'  to 
estimate  the  coefficients  of  a  closed-form  Lagrangian  dynamics  formulation.  The  10 
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inertial  parameters  of  each  link  are  lumped  into  these  numerous  coefficients,  which 
are  redundant  and  susceptible  to  numerical  problems  in  estimation.  On  the  other 
hand,  every  coefficient  is  identifiable  since  these  coefficients  represent  the  actual 
degrees  of  freedom  of  the  robot.  By  sensing  torque  from  only  one  joint  at  a  time, 
their  algorithm  is  more  susceptible  to  noise  from  transmission  of  dynamic  effects  of 
distant  links  to  the  proximal  measuring  joints.  For  efficient  dynamics  computation, 
the  recursive  dynamics  algorithms  require  the  link  parameters  explicitly.  Though 
recoverable  from  the  Lagrangian  coefficients,  it  is  better  to  estimate  the  inertial 
parameters  directly.  Though  this  algorithm  was  implemented  on  a  PUMA  robot, 
it  is  difficult  to  interpret  the  results  because  of  dominance  of  the  dynamics  by  the 
rotor  inertia  and  friction. 

Mukerjee  (1984;  Mukerjee  and  Ballard,  1985)  directly  applied  his  load  identifi¬ 
cation  method  to  link  identification,  by  requiring  full  force-torque  sensing  at  each 
joint.  Instrumenting  each  robot  link  with  full  force-torque  sensing  seems  imprac¬ 
tical,  and  is  actually  unnecessary  given  joint  torque  sensing  about  the  rotation 
axis.  Partially  as  a  result,  he  does  not  address  the  issue  of  unidentifiability  of 
some  inertial  parameters.  Also,  he  did  not  verify  his  algorithm  by  simulation  or  by 
implementation. 

Olsen  and  Bekey  (1985)  presented  a  link  identification  procedure  using  joint 
torque  sensing  and  special  test  motions  with  single  joints.  The  unidentifiability 
of  certain  inertial  parameters  was  not  resolved,  and  the  least  squares  estimation 
procedure  written  as  a  generalized  inverse  would  fail  because  of  linear  dependence  of 
some  of  the  inertial  parameters.  Again,  their  procedure  was  not  tested  by  simulation 
or  by  actual  implementation  on  a  robot  arm. 

Neuman  and  Khosla  (1985)  developed  a  hybrid  estimation  procedure  combining 
a  Newton-Euler  and  a  Lagrange-Euler  formulation  of  dynamics.  Simulation  results 
for  a  three  degree-of-freedom  cylindrical  robot  were  presented,  and  the  unidentifi¬ 
ability  of  certain  inertial  components  was  addressed.  For  some  reason  they  state 
link  mass  must  be  known  for  a  linear  estimation  procedure,  but  such  a  restriction 
does  not  exist  with  the  method  of  this  Chapter.  Though  planning  to  work  with  the 
CMU  DDArm  II,  they  have  not  yet  presented  experimental  results. 

Khalil,  Gautier,  and  Kleinfinger  (1986)  used  a  Lagrange  formulation  in  pre¬ 
senting  an  identification  model  for  link  inertial  parameters.  They  addressed  the 
unidentifiability  of  some  parameters,  and  used  it  to  regroup  the  dynamic  param- 


eters  and  simplify  computation.  However,  they  did  not  try  any  estimation  using 
their  model. 

Armstrong,  Khatib,  and  Burdick  (1986)  measured  the  inertial  properties  of  a 
PUMA  560  robot  by  counter-balancing  the  disassembled  parts.  This  is  an  alterna¬ 
tive  approach  to  estimation,  but  is  very  tedious.  Also  the  cross  terms  of  the  inertia 
matrix  cannot  be  obtained  in  this  way. 


3.2  Estimation  Procedure 

3.2.1  Formulation  of  Newton-Euler  Equations 


In  Chapter  2,  the  Newton-Euler  equations  for  a  rigid  body  load  were  formulated 
to  be  linear  in  the  unknown  inertial  parameters.  Then  simple  linear  least  squares 
method  was  used  to  estimate  those  parameters.  By  treating  each  link  of  a  manipula¬ 
tor  as  a  load,  this  formulation  can  be  extended  to  the  link  estimation  problem.  The 
differences  in  the  equations  are  that  only  one  component  of  force  or  torque  is  sensed 
and  that  the  forces  and  torques  from  distal  links  are  summed  and  transmitted  to 
the  proximal  joints. 

Consider  a  manipulator  with  n  joints  (Figure  3.1).  Each  link  *  has  its  own  local 
coordinate  system  P<  fixed  in  the  link  with  its  origin  at  joint ».  The  joint  force  and 
torque  due  to  the  movement  of  its  own  link  can  be  expressed  by  simply  treating  the 
link  as  a  load  and  applying  the  equations  from  Chapter  2  for  load  identification: 
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or  more  compactly, 


wu  =  Ai4>i 
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where  w,y  is  the  wrench  (vector  of  forces  and  torques)  at  joint  t  due  to  movement  of 
link  j  alone.  A,-  is  the  kinematic  matrix  that  describes  the  motion  of  link  *  and  is 
the  vector  of  unknown  link  inertial  parameters.  All  of  the  quantities  are  expressed 
in  the  local  joint  t  coordinate  system. 

The  total  wrench  wy  at  joint  i  is  the  sum  of  the  wrenches  w,y  for  all  links  j 
distal  to  joint  i: 

N 

W,  =  Y,  Wij  (3-2) 

J =»' 

Each  wrench  w,y  at  joint  i  is  determined  by  transmitting  the  distal  wrench  Wyy 
across  intermediate  joints.  This  is  a  function  of  the  geometry  of  the  linkage  only. 
The  forces  and  torques  at  neighboring  joints  are  related  by 
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(3.3) 


Wi.i+i  =  T,  w<+lif+1 

where 


(3.4) 


R,=  the  rotation  matrix  rotating  the  link  *  +  1  coordinate  system  to  the  link  i 
coordinate  system. 

8,-=  a  vector  from  the  origin  of  the  link  *  coordinate  system  to  the  link  t  +  1  coor¬ 
dinate  system,  and 

T,==  a  wrench  transmission  matrix. 

To  obtain  the  forces  and  torques  at  the  ith  joint  due  to  the  movements  of  the 
jtk  link,  these  matrices  can  be  cascaded: 


where  U,y 
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A  simple  matrix  expression  for  a  serial 
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kinematic  chain  (in  this  case  a  six  joint  arm)  can  be  derived  from  (3.2)  and  (3.5): 
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This  equation  is  linear  in  the  unknown  parameters,  but  the  left  side  is  composed 
of  a  full  force-torque  vector  at  each  joint.  Since  only  the  torque  about  the  joint  axis 
can  usually  be  measured,  each  joint  wrench  must  be  projected  onto  the  joint  rotation 
axis  (typically  [0,0,1]  in  internal  coordinates),  reducing  (3.6)  to 


r  =  KV> 


(3.7) 


where  =  [0, 0, 0, 0, 0,  lj-w,  is  the  joint  torque  of  the  ith  link,  0  =  [^,  ^2,  ^3,  ^4,  ^B,  ^6]r, 
and  Kij  =  [0, 0, 0, 0, 0,  l]  •  U,y  when  the  corresponding  entry  in  (3.6)  is  nonzero.  For 
an  n-link  manipulator,  risanxl  vector,  ^  is  a  lOn.  x  1  vector,  and  K  is  a  n  x  lOn 
matrix. 

3.2.2  Estimating  the  Link  Parameters 

Equation  (3.7)  represents  the  dynamics  of  the  manipulator  for  one  sample  point. 

As  with  load  identification,  (3.7)  is  augmented  using  N  data  points: 


K  = 

'  K(l)  ' 

r  = 

'  r(l)  ' 

K(N) 

r(N) 

Unfortunately,  one  cannot  apply  simple  least  squares  estimate: 

=  (KrK)-'KTr 


(3-8) 


because  KTK  is  not  invertible  due  to  loss  of  rank  from  restricted  degrees  of  free¬ 
dom  at  the  proximal  links  and  the  lack  of  full  force-torque  sensing.  Some  inertial 
parameters  are  completely  unidentifiable,  while  some  others  can  only  be  identified 
in  linear  combinations. 
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Two  different  approaches  were  used  to  solve  the  above  rank  deficient  problem. 
The  simplest  is  ridge  regression  (Marquardt  and  Snee,  1975),  which  makes  KrK 
invertible  by  adding  a  small  number  d  to  the  diagonal  elements: 

*  =  (KrK  +  dI10n)_1KrT  (3.9) 

The  estimates  are  nearly  optimal  if  d  «  Amin(KrK),  where  Am,„  is  the  smallest 
non-zero  eigenvalue  of  KTK. 

Another  approach  expresses  the  dynamics  in  terms  of  a  reduced  set  of  inertial 
parameters  that  are  independently  identifiable  and  that  allow  the  application  of  a 
straight  least  squares  estimate.  This  reduced  set  can  be  generated  either  by  exami¬ 
nation  of  the  closed  form  dynamic  equations  for  linear  combinations  of  parameters, 
or  by  application  of  singular  value  decomposition.  Both  methods  were  applied  and 
the  results  checked  against  each  other.  The  closed  form  equations  were  derived 
with  the  aid  of  MACSYMA  (Mathlab  Group,  1983)  for  the  MIT  Serial  Link  Direct 
Drive  Arm,  since  for  3  degrees  of  freedom  the  dynamic  equations  in  closed  form  are 
already  quite  complicated.  The  results  are  summarized  in  Appendix  1  in  terms  of 
15  essential  variables;  made  explicit  are  both  the  unidentifiable  parameters  and  the 
parameters  identifiable  only  in  linear  combinations. 

A  far  less  complicated  method  that  can  be  applied  rather  automatically  to  any 
manipulator  kinematic  structure  is  singular  value  decomposition  of  K  in  (3.8),  yield¬ 
ing  (Golub  and  Van  Loan,  1983) 

K  =  U£Vr 

where  £  =  diag{<r<}  and  U  and  VT  are  orthogonal  matrices.  For  each  column  of 
V  there  corresponds  a  singular  value  tr,  which  if  not  zero  indicates  that  the  linear 
combination  of  parameters,  'vjxj),  is  identifiable.  The  unidentifiable  parameters  will 
have  zero  singular  values  associated  with  them.  Since  K  is  a  function  only  of  the 
geometry  of  the  arm  and  the  commanded  movement,  it  can  be  generated  exactly 
by  simulation  rather  than  by  actually  moving  the  real  arm  and  recording  data  with 
the  concomitant  and  inevitable  noise.  For  completely  unidentifiable  parameters, 
the  corresponding  columns  of  K  can  be  deleted  without  affecting  r.  For  parameters 
identifiable  in  linear  combinations,  all  columns  except  one  in  a  linear  combination 
can  also  be  deleted.  The  resulting  smaller  KTK  matrix  will  be  invertible,  and  (3.8) 


Figure  3.2:  The  link  coordinate  system  for  DDARM 
can  be  used  to  estimate  the  reduced  set  of  parameters. 

3.3  Experimental  Results 

Link  estimation  was  implemented  on  the  MIT  Serial  Link  Direct  Drive  Arm  As 
discussed  in  Chapter  1,  the  ideal  rigid  body  dynamics  is  a  good  model  for  this  arm, 
uncomplicated  by  joint  friction  or  backlash  typical  of  other  manipulators.  Hence  the 
fidelity  of  this  manipulator’s  dynamic  model  suits  estimation  well.  The  coordinate 
system  for  this  arm  is  defined  in  Figure  3.2.  A  set  of  inertial  parameters  is  available 
for  the  arm  (Table  3.1)),  determined  by  modeling  with  a  CAD/CAM  database  (Lee, 
1983).  These  values  may  not  be  accurate  because  of  inevitable  modeling  errors,  but 
they  can  serve  as  a  point  of  comparison  for  the  estimation  results. 

Joint  1  is  presently  capable  of  an  angular  acceleration  of  1150  deg/ sec7,  joints  2 
and  3  in  excess  of  6000  deg /sec2.  In  comparison,  joint  1  of  the  PUMA  600  has  a  peak 
acceleration  of  130  deg/ sec1  and  joint  4  at  the  wrist  260  deg /sec2.  Joint  position  is 
measured  by  a  resolver  and  joint  velocity  by  a  tachometer.  The  tachometer  output 
is  of  high  quality  and  leads  to  good  acceleration  estimates  after  differentiation. 
The  accuracy  of  the  acceleration  estimates  plus  high  angular  accelerations  greatly 
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improves  inertia  estimation. 

The  joint  torques  are  computed  by  measuring  the  currents  of  the  3  phase  wind¬ 
ings  of  each  motor  (Asada,  Youcef-Toumi,  and  Lim,  1984).  For  the  three  phase 
brushless  permanent  magnet  motors  of  the  direct  drive  arm,  the  output  torque  is 
related  to  the  currents  in  the  windings  by: 

r  =  ifx(/osin(n0  +  of  f set)  +  7*  sin(nfl  +  120  +  offset)  +  Ie  sin(n0  +  240  4-  of  f  set)) 

(3.10) 

where 

60°  for  Joint  1 
offset1  =  •  —33.3  for  Joint  2 
—  108  for  Joint  3 

The  torque  constant  K?  for  each  motor  is  calibrated  statically  by  measuring  the 
force  produced  by  the  motor  torque  at  the  end  of  a  known  lever  arm.  As  in  load 
identification  experiments,  the  force  is  measured  using  a  Barry  Wright  Company 
Astek  FS6-120A-200  6-axis  force/torque  sensor.  Asada,  Youcef-Toumi,  and  Lim 
(1984)  have  found  that  for  a  motor  similar  to  the  motors  of  the  DDARM,  the 
torque  versus  current  relationship  was  non-linear,  especially  for  small  magnitudes 
of  torques,  and  also  varied  as  a  function  of  the  rotor  position.  However,  for  the 
results  presented  in  this  paper,  the  nonlinear  effects  were  ignored  since  substantial 
portions  of  the  movements  in  the  experiments  required  large  magnitudes  of  torques. 
Since  the  least  squares  algorithm  minimizes  the  square  of  the  error,  torque  errors 
for  torques  of  small  magnitudes  do  not  affect  the  estimates  very  much. 

For  the  estimation  results  presented,  600  data  points  were  sampled  while  the 
manipulator  was  executing  3  sets  of  fifth  order  polynomial  trajectories  in  joint 
space.  The  specifications  of  the  trajectories  were: 

1.  (330,  289.1,  230)  to  (80,  39.1,  -10)  degrees  in  1.3s, 

2.  (330,  269.1,  -30)  to  (80,  19.1,  220)  degrees  in  1.3s, 

3.  (80,  269.1,  -30)  to  (330,  19.1,  220)  degrees  in  1.3s, 

Since  KTK  in  (3.9)  is  singular,  estimates  for  the  30  unknowns  are  computed  by 
adding  a  small  number  ( d  =  10.0  <<  X  min(KrK)  —  3395.0)  to  the  diagonal  ele¬ 
ments  of  KtK. 
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Parameters 

Link  1 

Link  2 

Link  3 

m(Kg) 

67.13 

19.67 

mcx(Kg  •  m) 

0.0 

0.0 

TTXCy 

2.432 

3.4081 

0.0 

me. 

35.8257 

16.6505 

0.3268 

I„(Kgm J) 

23.1568 

7.9088 

0.1825 

0.0 

0.0 

0.0 

h. 

-0.3145 

0.0 

-0.0166 

Iyy 

20.4472 

7.6766 

0.4560 

Ar» 

-1.2948 

-1.5036 

0.0 

0.7418 

0.6807 

0.3900 

Table  3.1:  CAD-modeled  inertial  parameters. 


Parameters 

Link  1 

Link  2 

Link  3 

m{Kg) 

0.0* 

0.0* 

mcx(Kg  •  m) 
mcy 

0.0* 

-0.1591 

0.0* 

0.6776f 

0.0315 

mct 

0.0* 

0.0* 

-1.00871 

hz{Kgm 2) 

0.0* 

4.1562f 

1.5276f 

hv 

0.0* 

0.3894 

-0.0256 

/*, 

0.0* 

0.0118 

0.0143 

Iyv 

0.0* 

5.2129f 

1.8967f 

h* 

0.0* 

-0.60501 

-0.0160 

/„ 

9.33598f 

-0.8194f 

0.3568 

Table  3.3:  Parameters  in  linear  combinations  (J2  =  0.462  m.) 

Typical  results,  obtained  using  ridge  regression  method,  are  shown  in  Table  3.2. 
Parameters  that  cannot  be  identified  because  of  constrained  motion  near  the  base 
are  denoted  by  0.0*.  The  first  nine  parameters  of  the  first  link  are  not  identifiable 
because  this  link  has  only  one  degree  of  freedom  about  its  z  axis.  These  nine 
parameters  do  not  matter  at  all  for  the  movement  of  the  manipulator  and  thus  can 
be  arbitrarily  set  to  0.0. 

Other  parameters  marked  by  (f)  can  only  be  identified  in  linear  combinations, 
indicated  explicitly  in  Table  3.3.  The  ridge  regression  automatically  resolves  the 
linear  combinations  in  a  least  squares  sense.  It  can  be  seen  that  the  estimated  sums 
roughly  match  the  corresponding  sums  inferred  from  the  CAD-modeled  parameters, 
but  the  sizeable  discrepancy  indicates  that  one  parameter  set  may  be  more  accurate 
than  the  other. 

To  verify  the  accuracy  of  the  estimated  and  the  modeled  parameters,  the  mea¬ 
sured  joint  torques  are  compared  to  the  torques  computed  from  the  above  two 
sets  of  parameters  using  the  measured  joint  kinematic  data.  As  shown  in  Figure 
3.3,  the  estimated  torques  match  the  measured  torques  very  closely.  The  torques 
computed  from  the  CAD/CAM  modeled  parameters  do  not  match  the  measured 
torques  as  closely.  This  comparison  verifies  qualitatively  that  for  control  purposes 
the  estimated  parameters  are  in  fact  more  accurate  than  the  modeled  parameters. 
However,  as  in  Chapter  2,  one  cannot  make  conclusion  on  the  absolute  accuracy  of 
the  estimates  on  the  basis  of  the  plots  in  Figure  3.3.  The  plots  only  tell  us  that  one 
can  predict  the  joint  torques  well. 


Since  the  purpose  of  obtaining  the  estimates  was  to  improve  the  control  perfor¬ 
mance,  the  best  test  for  verifying  the  goodness  of  the  estimates  is  to  use  them  in 
robot  controllers.  In  Chapter  4,  the  estimated  inertial  parameters  will  be  used  in 
studying  the  effectiveness  of  different  control  algorithms. 

3.4  Discussion 

Good  estimates  of  the  link  inertial  parameters  were  obtained  as  determined  from  the 
match  of  predicted  torques  to  measured  torques.  The  potential  advantage  of  this 
movement-based  estimation  procedure  for  increased  accuracy  as  well  as  convenience 
was  demonstrated  by  the  less  accurately  predicted  torques  based  on  the  CAD- 
modeled  inertial  parameters. 

The  inaccuracy  of  the  CAD-modeled  parameters  is  due  to  several  sources.  The 
links  and  the  motors  are  complicated  and  computing  the  inertial  parameters  from 
the  schematic  drawing  of  the  manipulator  is  bound  to  contain  modelling  errors. 
For  the  MIT  Serial  Link  Direct  Drive  Arm,  the  masses  and  moments  of  inertia 
are  dominated  by  the  large  motors  at  the  joints.  The  modelling  of  the  inertial 
properties  of  these  motors  are  difficult  since  they  are  made  of  complicated  parts 
such  as  the  stator  windings.  Also,  the  links  can  be  attached  to  the  rotor  axes  at 
arbitrary  positions  by  the  assembler,  introducing  uncertainty  in  the  CAD-modeled 
parameters. 

It  is  possible  that  the  inaccuracy  of  the  CAD-modeled  parameters  is  exagger¬ 
ated,  since  the  same  sensors  that  were  used  in  the  estimation  are  being  used  to 
compare  the  CAD-modeled  parameters  to  the  dynamically  estimated  parameters. 
Presumably  a  systematic  error  in  the  sensors,  such  as  a  mis-calibration  of  motor 
torque  constants  Kt ,  would  be  reflected  in  the  dynamically  estimated  parameters. 
This  would  lead  to  a  judgment  of  better  match  with  these  estimated  parameters, 
even  though  the  CAD-modeled  parameters  could  conceivably  be  the  more  accurate. 
Ideally  an  independent  measuring  procedure  such  as  weighing  and  counterbalancing 
should  be  used  to  resolve  this  point,  but  this  was  not  tried. 

With  regard  to  errors  in  the  motor  torque  constant,  the  motors  were  calibrated 
with  a  commercial  force/torque  sensor,  and  it  is  expected  that  errors  in  this  cali¬ 
brating  device  are  very  small.  Problems  of  a  dead  zone  near  zero  torque  and  torque 
ripple  (Asada,  Youcef-Toumi,  and  Lim,  1984)  are  not  considered  to  be  significant 
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Figure  3.3:  The  measured,  CAD-modelled,  and  the  estimated  joint  torques 
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because  of  the  large  torques  used  in  this  study.  Other  sources  of  error  are  the  same 
as  discussed  in  the  previous  chapter,  and  are  not  repeated  here. 

Even  supposing  that  there  are  possible  errors  in  the  sensors  or  kinematic  varia¬ 
tions  due  to  assembly,  the  importance  of  the  dynamic  estimation  of  the  link  inertial 
parameters  is  actually  emphasized.  The  controller  must  deal  with  the  robot  kine¬ 
matics  and  sensor  calibration  as  they  exist,  and  the  estimated  model  will  accom¬ 
modate  kinematic  variations  and  cancel  sensor  calibration  error. 

3.4.1  Identifiability  of  Inertial  Parameters 

There  are  three  groups  of  inertial  parameters:  fully  identifiable,  identifiable  in  linear 
combinations,  and  completely  unidentifiable.  Membership  of  a  parameter  in  a  group 
depends  on  the  manipulator’s  particular  geometry.  As  shown  in  Table  3.2  and 
Appendix  1  for  the  MIT  Serial  Link  Direct  Drive  Arm,  the  30  inertial  parameters 
are  grouped  into  the  following  categories: 

1.  fully  identifiable.  m2CIa,  7Zy2  >  fuji  ^3Cys»  7Xys,  fuji  7yXS,  7XXJ 

2.  identifiable  in  linear  combinations:  u  ,  nijCyj,  /,J,  ,  -f yy 2  ,  -fyjtj  )  1**2  >  ^*3»  ^J®*jl 

7***>  ly vs 

3.  completely  unidentifiable:  mlt  m^*,,  m^y,,  m^,,,  7XXl,  7XVl,  7yy, ,  7yXl, 

,n2('*2 

Some  link  inertial  parameters  are  unidentifiable  because  of  restricted  motion 
near  the  base  and  the  lack  of  full  force-torque  sensing  at  each  joint.  For  the  first 
link,  rotation  is  only  possible  about  its  z  axis.  Suppose  full  force-torque  sensing  is 
available  at  joint  1.  It  can  be  seen  from  (3.1)  that  7XXl,  7IVl,  and  IVVl  are  unidentifi¬ 
able  because  they  have  no  effect  on  joint  torque.  Since  the  gravity  vector  is  parallel 
to  the  z  axis,  cXl  is  also  unidentifiable.  If  it  is  now  supposed  that  only  torque  about 
the  z  axis  can  be  sensed,  then  all  inertial  parameters  for  link  1  become  unidentifiable 
except  7XXJ. 

In  a  multi-link  robot  a  new  phenomenon  arises.  Some  parameters  can  only  be 
identified  in  linear  combinations,  because  proximal  joints  must  provide  the  torque 
sensing  to  identify  fully  the  parameters  of  each  link.  Certain  parameters  from  distal 


links  are  carried  down  to  proximal  links  until  a  link  appears  with  a  rotation  axis  ori¬ 
ented  appropriately  for  completing  the  identification.  In  between,  these  parameters 
appear  in  linear  combinations  with  other  parameters.  This  partial  identifiability 
and  the  difficulty  of  analysis  become  worse  as  the  number  of  links  are  increased. 

The  ridge  regression  automatically  resolves  the  linear  combinations  in  a  least 
squares  sense,  which  makes  these  inertial  parameters  appear  superficially  different 
from  those  derived  by  CAD  modeling.  This  is  an  approximation  to  computing  the 
pseudo  inverse  to  solve  the  rank  deficient  least  squares  problem. 

Although  not  as  simple  as  ridge  regression,  singular  value  decomposition  of 
K  in  (3.8)  to  determine  the  minimal  number  of  inertial  parameters  is  attractive 
since  it  allows  reformulating  the  dynamics  with  identifiable  parameters  only.  The 
procedure  isolates  several  sets  of  parameters  whose  linear  combinations  within  each 
set  are  identifiable.  The  linear  combinations  can  be  reduced  by  consistently  setting 
certain  parameters  in  these  sets  to  zero,  leaving  only  one  non-zero  parameter  in  each 
set;  for  example,  zeroing  m3,  m3cls,  /«,,  and  /,*,  leaves  IVWa, 
and  m2cyj  as  identifiable  parameters.  The  unidentifiable  parameters  can  also  be 
set  to  zero.  Finally,  what  remains  is  a  reduced  full-rank  KTK  matrix  of  dimension 
15  x  15. 


Chapter  4 


Feedforward  and  Computed 
Torque  Control 


4.1  Introduction 

The  accuracy  of  the  manipulator  dynamic  model  impinges  on  the  performance  of  a 
dynamic  controller  using  that  model.  Since  friction  is  negligible  for  the  direct  drive 
arms,  and  presuming  that  one  has  good  control  of  joint  torques,  the  issue  of  accuracy 
reduces  to  how  well  the  inertial  parameters  of  the  rigid  links  are  known.  In  Chapters 
2  and  3,  an  algorithm  was  developed  for  estimating  these  inertial  parameters  for 
any  multi-link  robot  as  a  result  of  movement,  and  the  inertial  parameters  of  the 
MIT  Serial  Link  Direct  Drive  Arm  were  estimated.  This  chapter  presents  results 
of  utilizing  the  estimated  model  to  control  the  robot  by  both  off-line  (feedforward) 
and  on-line  (computed  torque)  computation  of  the  joint  torques. 

Two  sets  of  experiments  were  performed  with  the  MIT  Serial  Link  Direct  Drive 
Arm  (DDARM)  involving  a  subset  of  proposed  control  strategies.  The  first  set  of 
experiments  is  based  on  a  hybrid  control  system.  There  is  an  independent  analog 
servo  for  each  joint  with  the  position  and  velocity  references,  and  feedforward  com¬ 
mands  generated  by  a  microprocessor.  Since  most  commercial  arms  are  controlled 
by  simple  independent  PID  controller  for  each  joint,  an  independent  PD  controller 

This  chapter  is  a  revised  version  of  (An,  Atkeson,  Griffiths,  and  Hollerbach;  1986) 
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was  tested  on  this  arm  to  provide  a  baseline  for  comparison.  The  PD  controller  was 
augmented  by  feeding  forward  first  gravity  compensation  and  then  the  complete 
rigid  body  dynamics  to  ascertain  any  trajectory  following  improvements  attained 
by  taking  the  nonlinear  dynamics  more  fully  into  account.  The  second  set  of  ex¬ 
periments  shows  the  preliminary  results  of  digital  servo  implementation,  using  one 
Motorola  68000  based  microprocessor  to  control  all  the  joints  of  the  DDARM.  The 
on-line  computed  torque  approach  is  compared  to  the  PD  and  to  the  feedforward 
approaches  using  the  digital  servo. 

4.1.1  Control  Algorithms 

The  full  dynamics  of  an  n  degree-of-freedom  manipulator  are  described  by 

n  =  J(q)q  +  b(q,  q)  +  g(q)  +  f/r(q,  q)  (4.1) 

where  n  is  the  vector  of  joint  torques  (for  rotational  joints),  q  is  the  vector  of  joint 
angles,  J  is  the  inertia  matrix,  b  is  the  vector  of  coriolis  and  centripetal  terms,  g 
is  the  gravity  vector,  and  f/r  is  the  vector  of  friction  terms.  The  simplest  and  most 
common  form  of  robot  control  is  independent  joint  PD  control,  described  by 

n  =  K„(qd  —  q)  +  Kp(qd  —  q)  (4.2) 

where  qd  and  qd  are  the  desired  joint  velocities  and  positions,  and  Kp  and  Kt  are 
nxn  diagonal  matrices  of  position  and  velocity  gains. 

Feedforward  control  (Fig.  4.1)  augments  the  basic  PD  controller  by  providing  a 
set  of  nominal  torques  n //: 

n//(q<*,q<nqd)  =  J(qd)qd  +  b  (qd,qd)  +  g(q  <*)  +  f/>(qi,q<<)  (4.3) 

where  the  hat  (")  refers  to  the  modelled  values.  When  this  equation  is  combined 
with  (4.2),  the  feedforward  controller  results: 

n  =  ^/(qj, qd,  qd)  +  K„(qd  -  q)  +  Kp(qd  -  q)  (4.4) 

The  feedforward  term  n jf  can  be  thought  of  as  a  set  of  nominal  torques  which 
linearize  the  dynamics  (4.1)  about  the  operating  points  qd,qd,  and  qd.  Therefore, 
it  is  reasonable  to  add  the  linear  feedback  terms  K„(qd  —  q)  +  Kp(qd  —  q)  as  the 
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Figure  4.1:  Feedforward  controller 


control  for  the  linearized  system.  These  feedforward  terms  can  be  computed  off-line, 
since  they  are  function  of  the  parameters  of  the  desired  trajectory  only. 

On  the  other  hand,  the  computed  torque  controller  computes  the  dynamics  on¬ 
line,  using  the  sampled  joint  position  and  velocity  data.  The  control  equation  is: 

nct(q«i,q,q<i,q,q<i)  =  j(q)q’  +  b(q,q)+g(q)+f/r(q,q)  (4.5) 

where  q*  is  given  by, 


q*  =  qa  +  K^qj-q) +Kp(qd-q).  (4.6) 

If  the  robot  model  were  exact,  then  each  link  of  the  robot  is  decoupled,  and  the 
trajectory  error  goes  to  zero.  Gilbert  and  Ha  (1984)  have  shown  that  the  computed 
torque  control  method  is  robust  to  small  modelling  errors. 

Previously,  Liegeois,  Fournier,  and  Aldon  (1980)  suggested  feedforward  control 
as  an  alternative  to  the  on-line  computation  requirements  of  computed  torque  con¬ 
trol,  although  they  did  not  present  any  experimental  results.  Golla,  Garg,  and 
Hughes  (1981)  discussed  different  linear  state-feedback  controller  using  a  linearized 
model  of  a  manipulator.  Asada,  Kanade,  and  Takeyama  (1983)  presented  some 
results  of  applying  a  feedforward  control  to  the  early  version  of  a  direct  drive  arm 
at  the  Robotics  Institute  of  CMU,  though  for  quite  slow  movements  and  for  in¬ 
ertial  parameters  derived  by  CAD  modeling.  The  computed  torque  method  has 
been  considered  by  several  researchers  (Paul,  1972;  Markiewicz,  1973;  Bejczy,  1974; 
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Luh,  Walker,  and  Paul,  1980b;  Gilbert  and  Ha,  1984).  Although  simulation  re¬ 
sults  have  been  presented,  there  has  been  very  few  published  report  on  the  actual 
implementation  of  this  controller,  mainly  due  to  the  lack  of  an  appropriate  manip¬ 
ulator  or  on-line  computational  facility.  Working  with  a  direct  drive  arm,  Khosla 
and  Kanade  (1986)  presented  performance  comparisons  of  of  the  computed  torque 
controller  to  the  PD  controller.  The  conclusion  from  their  experiments  is  similar  to 
the  conclusion  of  this  chapter. 

In  this  paper,  the  feedforward  control  is  first  used  as  a  method  of  evaluating 
the  accuracy  of  the  estimates  of  the  inertial  parameters  of  the  links,  and  compare 
performance  of  the  feedforward  controller  to  several  other  simpler  control  methods 
for  high  speed  movements.  Then,  I  present  some  preliminary  results  on  the  im¬ 
plementation  of  a  computed  torque  controller,  again  using  the  estimated  inertial 
parameters  of  the  links. 

4.2  Robot  Controller  Experiment 

4.2.1  Feedforward  Controller 

In  this  section,  performances  of  several  different  controllers  for  full  motion  of  the 
DDARM  is  evaluated  using  a  hybrid  controller  (An,  Atkeson,  and  Hollerbach,  1986). 
The  reference  positions  and  velocities,  and  the  feedforward  torques  are  generated  by 
a  microprocessor  and  input  to  three  independent  joint  analog  servos.  Evaluations 
of  the  following  five  control  methods  are  performed  with  high  speed  movements  of 
all  three  joints  of  the  manipulator: 

1.  PD  controller  with  position  reference  only: 

n  =  — K„q  +  Kp{qd  -  q) 

2.  PD  controller  with  position  reference  and  feedforward  of  gravity  torques: 

n  =  g(9d)  ~  Kuq  +  Kp(qd  -  q) 

3.  PD  controller  with  position  and  velocity  references: 
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4.  PD  controller  with  position  and  velocity  references  plus  feedforward  of  gravity 
torques: 

n  =  g(qd)  +  Ku(qi  -  q)  +  K^q*  -  q) 

5.  PD  controller  with  position  and  velocity  references  plus  feedforward  of  full 
dynamics: 

n  =  J(q<i)q<i  +  b(q<i»  q'i)  +  g(q<<)  +  Kv(qj  -  q)  +  Kp(qrf  -  q) 

In  these  experiments,  friction  was  neglected.  The  nominal  position  and  velocity 
gains  were  adjusted  experimentally  to  achieve  high  stiffness  and  overdamped  chara- 
teristics  without  the  feedforward  terms. 

A  fifth  order  polynomial  in  joint  space  was  used  to  generate  the  reference  trajec¬ 
tory.  The  joints  moved  from  (80°, 269.1°,— 30°)  to  (330°,  19.1°, 220°)  in  1.3s,  with 
peak  velocities  of  360  deg /sec  and  the  peak  accelerations  of  854  deg /sec1  for  each 
joint.  The  reference  trajectory  for  Joint  1  is  shown  in  Figure  4.2.  For  control  meth¬ 
ods  (2),  (4),  and  (5),  the  estimates  of  the  link  inertial  parameters  given  in  Chapter  3 
(An,  Atkeson,  and  Hollerbach;  1985)  were  used  to  compute  the  feedforward  torques. 

The  trajectory  errors  for  the  above  5  controllers  are  shown  on  Figure  4.3.  The 
errors  for  the  first  controller  are  very  large  and  are  out  of  the  graph  range.  Adding 
a  gravity  feedforward  term  does  not  help  very  much,  and  the  trajectory  errors  for 
Controller  2  are  also  very  large.  This  was  expected  since  gravity  feedforward  is  a 
static  correction  to  Controller  1,  and  the  dynamic  effects  dominate  the  response  for 
high  speed  movements.  Modifying  the  first  controller  by  adding  a  velocity  reference 
signal  improved  the  response  greatly.  As  with  Controller  2,  adding  a  gravity  feed¬ 
forward  term  did  not  reduce  the  trajectory  errors  very  much,  and  influenced  mainly 
the  steady  state  errors  for  joints  2  and  3. 

The  full  feedforward  controller  reduced  the  trajectory  errors  significantly  for 
Joints  1  and  2,  with  peak  errors  of  only  0.33°  and  0.64°,  respectively.  For  Joint  3, 
the  feedforward  torques  did  not  help  because  of  the  light  inertia  and  the  dominance 
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Figure  4.3:  Trajectory  errors  of  the  5  controllers  for  full  1.3s  motion 
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of  unmodelled  dynamics  in  the  motor  and  in  bearing  friction.  The  high  feedback 
gains  make  this  joint  somewhat  robust  to  these  unmodelled  dynamics;  yet,  the 
trajectory  errors  could  not  be  reduced  below  1.4°  with  the  feedforward  torques 
based  on  the  ideal  rigid  body  model  of  the  link. 


4.2.2  Computed  Torque  Controller 


In  this  section,  some  preliminary  results  are  presented  for  the  computed  torque 
method  implemented  on  the  DDARM.  In  this  implementation,  the  analog  servos 
are  disabled,  and  the  feedback  computation  is  done  digitally  by  one  Motorola  68000 
based  microprocessor  using  scaled  fixed-point  arithmetic.  Written  in  the  C  lan¬ 
guage,  the  controller,  including  the  full  computation  of  the  robot  dynamics,  runs 
at  a  133  Hz  sampling  frequency.  Although  further  improvements  in  computation 
time  are  possible,  this  speed  was  adequate  in  demonstrating  the  efficacy  of  dynamic 
compensation.  The  details  of  this  implementation  are  discussed  in  (Griffiths,  1986) . 

A  similar  fifth  order  polynomial  trajectory  as  in  the  previous  section  was  used 
for  this  experiment.  Figure  4.4  shows  the  trajectory  errors  for  three  controllers: 
the  digital  PD  controller,  the  feedforward  controller  using  a  digital  servo,  and  the 
computed  torque  controller.  The  computed  torque  and  the  feedforward  controllers 
both  show  a  significant  reduction  in  tracking  errors  for  Joints  1  and  2  compared 
with  the  PD  control,  with  no  clear  distinction  between  feedforward  and  computed 
torque.  With  the  addition  of  dynamic  component,  the  peak  tracking  errors  for 
Joint  1  are  reduced  from  4.4°  to  2.2°  and  for  Joint  2,  from  3.5°  to  2.0°.  As  before, 
the  trajectory  errors  for  Joint  3  were  not  reduced  by  the  computed  torque  or  the 
feedforward  controller.  Again,  this  seems  to  indicate  that  the  model  for  the  third 
link  may  not  be  very  good. 

The  trajectory  tracking  performance  of  the  computed  torque  controller  is  not  as 
good  as  that  of  the  analog  feedforward  controller  of  the  previous  section.  The  main 
reason  for  this  is  the  slow  sampling  frequency  (133  Hz)  of  the  digital  controller,  as 
compared  to  the  1  KHz  sampling  frequency  at  which  the  reference  inputs  were  given 
to  the  analog  servos.  Improvements  in  the  computation  time  should  also  improve 
the  tracking  performance  of  the  computed  torque  controller. 
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4.3  Conclusions 


I 

In  this  chapter,  I  presented  some  experimental  results  of  using  an  estimated  dy¬ 
namic  model  of  the  manipulator  for  dynamic  compensation  via  feedforward  and 
computed  torque  control  methods.  The  results  indicate  that  dynamic  compensa¬ 
tion  can  improve  trajectory  accuracy  significantly  and  that  the  estimated  rigid  body 
model  of  the  manipulator  is  quite  accurate  and  adequate  for  control  purposes  for 
Joints  1  and  2.  The  unmodelled  dynamics  of  the  light  third  link,  including  the 
motor  dynamics  and  friction,  are  significant  and  yield  larger  trajectory  errors  than 
at  the  other  two  joints.  Therefore,  for  Joint  3,  it  may  be  necessary  to  use  a  more 
complete  model  to  improve  trajectory  following. 

The  results  of  the  digital  implementation  of  the  feedforward  and  computed 
torque  controllers  were  not  as  good  as  the  hybrid  feedforward  controller.  This 
indicates  that  if  a  robot  was  being  used  solely  for  free  space  movements  without 
significant  variation  of  its  loads,  then  a  hybrid  controller  using  an  independent  ana¬ 
log  servo  for  each  joint  may  be  quite  adequate.  A  hybrid  controller,  however,  is  not 
flexible,  and  cannot  handle  varying  loads  or  interactions  with  the  environment. 
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Chapter  5 


Dynamic  Stability  Issues  in  Force 
Control 


5.1  Introduction 

It  is  necessary  to  make  robots  interact  with  their  environments  to  make  them  ver¬ 
satile.  This  is  especially  true  for  assembly  applications  with  tight  tolerances  since 
robots  may  not  have  the  necessary  positional  accuracy  or  have  some  unmodelled 
misalignment.  Vision  will  be  a  part  of  the  solution  for  such  problems.  But  even 
then,  robots  need  to  interact  with  the  environments  in  a  stable  way.  This  is  the 
subject  of  force  or  compliance  control  studies  that  have  been  getting  much  atten¬ 
tion  recently  in  the  robotics  field.  However,  in  all  of  the  active  continuous  feedback 
implementations  mentioned  in  Chapter  1,  researchers  have  found  that  their  systems 
could  not  deal  with  stiff  environments  satisfactorily  (Whitney,  1985;  Caine,  1985). 
Previous  implementations  have  been  either  unstable  or  very  slow.  The  slow  perfor¬ 
mance  is  due  to  high  joint  damping  required  to  maintain  stability  during  contact 
between  the  tip  of  the  robot  and  its  work  piece,  i.e.  during  interaction  with  a 
stiff  environment.  Without  such  high  damping,  researchers  have  observed  a  very 
unstable  behavior  in  a  force-controlled  robot. 

Stability  is  a  condition  that  must  be  satisfied  with  any  control  systems.  Yet, 
often  in  the  area  of  robotics,  the  stability  problems  have  been  neglected  and  treated 
only  as  an  afterthought.  Only  during  the  last  two  years  have  there  been  papers 


discussing  the  stability  problems  associated  with  force  control  (Whitney,  1985; 
Roberts,  1984,  1985;  Kazerooni,  1985,  1986a,  1986b,  1986c;  Eppinger,  1986;  Wla- 
sich,  1986).  Both  Whitney  (1985)  and  Roberts  (1984)  presented  stability  analysis 
with  respect  to  the  sensor  stiffness  and  showed  that  a  soft  force  sensor  is  necessary 
in  order  to  achieve  a  stable  behavior  with  stiff  environments.  Kazerooni  (1985) 
has  presented  a  stability  analysis  and  design  method  for  impedance  controller  using 
eigen  structure  assignment.  But  his  algorithm  is  quite  complicated,  and  it  is  not 
clear  if  it  can  be  implemented  for  a  multi  degree  of  freedom  manipulator.  His  imple¬ 
mentation  results  have  so  far  been  on  a  single  joint  manipulator.  Wlassich  (1986) 
implemented  an  impedance  controller  on  a  two-link  experimental  manipulator.  Us¬ 
ing  the  impedance  control  method,  it  may  be  desired  to  make  the  manipulator 
behave  as  if  it  were  a  smaller  mass  (or  inertia)  than  the  actual.  However,  Wlassich 
discovered  that  unless  the  desired  mass  was  larger  than  the  actual  mass,  the  ma¬ 
nipulator  became  unstable  against  a  stiff  environment.  Eppinger’s  analysis  (1986) 
is  similar  to  that  contained  in  Section  5.2,  but  he  does  not  come  to  any  conclusions 
for  remedying  the  stability  problems. 

The  goal  of  this  and  the  next  two  chapters  is  to  present  simple  analyses  to 
understand  the  stability  problems  associated  with  force  control  implementations 
and  then  to  present  some  design  methods  that  would  remedy  those  problems.  This 
chapter  focuses  on  the  dynamic  instabilities  that  are  caused  by  the  interaction  of  the 
dynamics  of  the  robot  with  its  environment.  Since  these  problems  occur  whether 
we  are  dealing  with  a  single  degree  of  freedom  manipulator  or  a  multi  degree  of 
freedom  manipulator,  a  simple  one  link  manipulator  is  used  in  the  analyses.  The 
analyses  are  then  verified  by  single  link  experiments  on  the  third  link  of  the  direct 
drive  arm.  Multi-link  cases  are  discussed  in  Chapters  6  and  7. 

After  presenting  the  dynamic  stability  problems  in  Section  5.2,  three  different 
solutions  are  suggested  to  improve  the  dynamic  stability  of  a  force-controlled  ma¬ 
nipulator.  The  first  method,  use  of  compliant  covering,  is  similar  to  the  results  and 
suggestions  of  Whitney  (1985)  and  Roberts  (1984).  The  second  method,  adaptation 
to  the  environment  stiffness,  is  new  and  has  not  been  reported  before.  The  third 
method,  use  of  joint  torque  control,  has  been  mentioned  by  Wu  and  Paul  (1980) 
and  by  Luh,  Fisher,  and  Paul  (1983);  but  their  results  were  used  mainly  to  reduce 
joint  friction  in  a  position  controlled  system,  and  not  in  active  force  control. 

As  mentioned  in  Chapter  1,  the  direct  drive  arm  is  an  ideal  device  for  testing 


Figure  5.1:  Model  of  the  robot  and  the  environment 

these  ideas  for  the  following  reasons: 

•  the  dynamics  are  ideal  since  there  is  little  friction  and  no  backlash, 

•  torques  to  the  motors  can  be  measured  and  controlled  accurately. 

5.2  Stability  Problems 

In  this  section,  I  will  discuss  the  dynamic  stability  problems  of  a  force-controlled 
robot.  Two  types  of  force  control  methods  are  used  as  examples,  although  the 
problems  are  general  and  not  limited  to  these  two.  At  first,  the  stability  problems 
will  be  analyzed  using  a  simple  model  of  the  robot  and  then  demonstrated  by 
experiments  on  the  direct  drive  arm. 

5.2.1  General  Stability  Analysis 

To  simplify  the  analysis,  I  will  consider  a  very  simple  model  of  the  robot  and  the 
environment  (Fig.  5.1).  The  robot,  in  contact  with  its  environment,  is  modelled  as 
a  mass  m,  and  the  environment  plus  the  wrist  force  sensor  is  modeled  as  a  simple 
spring  of  stiffness  kE.  Then  the  dynamic  equation  of  the  above  simplified  system  is, 

/  -  kEx  =  mx  (5.1) 
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Stiffness  Control  For  a  stiffness  controller  (Salisbury,  1980)  of  the  above 
model,  the  control  input  is  given  by, 

f=kP{xd  -  x)  +  ku{xd  -  x)  +  k,[kp{xd  -  x)  -  /„,)  ^  ^ 

=kp(xd  -  x)  +  ku[xd  -  x)  +  kf(kp(xd  -  x)  -  kEx) 

where  kp  is  the  desired  stiffness,  kv  is  the  velocity  gain,  and  k/  is  the  force  feedback 
gain.  Substituting  (5.2)  into  (5.1),  the  dynamics  of  the  total  controlled  system  is 
described  by, 

mi  +  kvx  +  (1  +  kf)(kp  +  kE)x  =  input  terms  (5.3) 

Impedance  Control  An  impedance  controller  (Hogan,  1985a,  1985b,  1985c) 
results  in  a  similar  form  as  (5.2),  but  with  kj  depending  on  the  desired  apparent 
mass.  The  control  equation  of  an  impedance  controller,  discussed  in  (Hogan,  1985b) 
for  the  simple  one  degree  of  freedom  robot,  is  as  follows: 

•  Desired  impedance: 


mdx  +  kv(x  -  xd)  +  kp(x  -  xd)  =  ftxt  (5.4) 

•  Control  Input: 

f=^(kr(xd  ~  *)  +  kv(xd  -  x))  +  (1  -  ,5  5) 

=  %(kp(x*  ~x)+  k«(xd  ~  x))  +  U  “  %)kEx- 
md  is  the  desired  mass,  ku  is  the  desired  damping,  and  kp  is  the  desired  stiffness. 
(5.5)  shows  that  the  force  gain  kf  is  directly  related  to  the  desired  mass  md. 

*/  =  (!"  ^-)  (5-6) 

md 

Therefore,  if  md  <  m,  then  kf  is  negative  and  large.  If  md  =  m,  then  the  force 
sensor  feedback  is  disabled.  If  md  >  m,  then  kf  is  small  but  positive.  This  positive 
force  feedback,  however,  does  not  cause  instability  because  of  the  —  kEx  term  already 
present  in  the  open  loop  system  (5.1).  The  total  system  with  the  above  impedance 
control  is  described  by 

n  TTX  t  ,  YY\  < .  *  \  . 

mi  H - kvx  +  — [kp  -I-  kE)x  =  input  terms 

md  md 


(5.7) 


Figure  5.2:  Error  model  for  unmodelled  dynamics 


This  equation  is  almost  identical  to  (5.3)  for  the  stiffness  controlled  system.  In  both 
cases,  the  closed  loop  equation  includes  a  position  term  x  which  is  multiplied  by 
the  effective  stiffness  of  the  environment  kE  and  the  force  feedback  gain  kj.  In  fact, 
this  form  of  equation  will  result  for  any  type  of  force  control  methods. 

The  system  described  by  (5.3)  or  (5.7)  is  a  stable  system  since  all  the  poles  have 
negative  real  parts  given  positive  gains,  k„  and  kp.  In  free  space  or  in  contact  with  a 
soft  environment,  kE  would  be  small,  and  the  manipulator  will  behave  satisfactorily. 
But  if  kE  is  large,  i.e.  kE»  kp,  the  system  will  be  undesirably  underdamped  if  kv 
was  computed  with  only  kp  in  mind,  neglecting  the  large  kE  term.  The  combination 
of  the  force  sensor  and  the  environment  is  approximately  a  very  stiff  spring,  and 
from  the  stability  point  of  view,  force  feedback  is  a  very  high  gain  position  feedback. 

Yet,  given  the  above  completely  linear  and  perfectly  modelled  system,  this  is  still 
a  stable  system  since  the  poles  are  still  on  the  left  hand  plane.  However,  real  systems 
are  non-linear,  and  they  are  never  perfectly  modelled.  In  such  real  situations,  the 
system  may  actually  be  unstable  in  presence  of  unmodelled  high  frequency  dynam¬ 
ics.  Figure  5.2  shows  the  nominal  closed-loop  model  of  a  force-controlled  system 
and  the  use  of  multiplicative  error  model  to  represent  the  unmodelled  dynamics. 
E(s)  is  not  completely  known  and  is  only  an  approximation  to  the  unmodelled  dy¬ 
namics.  In  a  single  input  single  output  system,  for  stability  robustness  with  the 
above  error  model  (Lehtomaki,  1982)  shown  in  Figure  5.2,  we  need: 

|£(s)|<fl +  £->(*)!  (5.8) 

This  relation  is  derived  in  Appendix  2  from  the  Nyquist  criterion  for  stability.  For 
my  simple  model  of  combined  manipulator-environment  system,  the  loop  transfer 
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Figure  5.3:  1 1  +  G  1(s)|  for  kv  =  250,  m  =  1,  kj  =  10,  and  A:p  =  1000;  Typical 

l*MI 

function  is: 

=  «*. +*,(!+*/)  (5.9) 

*  s2  +  kB 

The  plots  of  \E(s)\  and  |1  +  G_1(s)|  for  different  values  of  kB  are  shown  on  Figure 
5.3.  The  plot  of  |^(a)|  is  a  hypothetical  one,  but  its  shape  is  typical  of  the  high 
frequency  unmodelled  dynamics  (Kazerooni,  1985).  The  figure  shows  that  for  a 
small  kv,  which  still  gives  overdamped  response  in  free  space,  \I  +  G-1(s)|  has  a 
large  dip  in  the  high  frequency  region  for  large  values  of  kB.  Since  the  unmodelled 
dynamics  are  greater  for  higher  frequencies,  this  results  in  a  non-robust  system.  In 
fact,  with  E(s)  as  shown  on  the  figure,  [i?(s)|  >  |/+G-1(s)|  forfcjg  >  10s  Nt/m,  and 
this  system  then  may  be  unstable  for  contact  with  such  stiff  environments.  But  for 
a  soft  environment,  the  diagram  shows  that  the  system  remains  stable  in  presence 
of  unmodelled  dynamics.  Such  behaviors  have  been  noticed  by  the  researchers  who 
have  tried  implementing  force  control  for  a  manipulator. 

In  summary,  there  are  three  problems  affecting  stability  for  a  robot  interacting 
with  its  environments.  They  are: 

1.  force  sensor  feedback  is  essentially  high  gain  position  feedback, 

2.  there  are  always  unmodelled  high  frequency  dynamics  in  the  robot  system, 

and 


'V, 


ROBOT 


Figure  5.4:  2  mass  model  (flexibility) 


3.  the  robot  must  deal  with  stiff  environments. 

In  the  next  two  sections,  I  will  verify  this  general  simple  analysis  with  an  example 
and  also  by  implementation  on  the  MIT  Serial  Link  Direct  Drive  Arm. 

5.2.2  Example  of  Unmodelled  dynamics 

The  general  analysis  of  the  previous  subsection  is  verified  in  this  section  with  a 
slightly  more  complex  model  of  a  robot.  Even  single  degree  of  freedom  robots  are 
not  completely  rigid  and  there  are  always  flexible  modes  in  the  system.  Adding  a 
flexible  mode  to  the  simple  system  (Fig.  5.1),  a  fourth  order  model  of  the  robot  in 
contact  with  the  environment  is  shown  in  Figure  5.4. 

With  this  system,  if  a  stiffness  controller  or  any  other  force  controller  was 
designed  straightforwardly  neglecting  the  flexibility,  thus  treating  it  as  a  simple 
one  mass  system,  the  root  locus  of  the  closed-loop  system  with  stiff  environment 
[ks  —  8  x  10s  N/m)  would  behave  as  shown  in  Figure  5.5.  As  the  force  gain  kf  is 
increased,  the  poles  move  to  the  right  half  plane  and  the  system  becomes  unstable. 
When  the  robot  is  in  contact  with  a  soft  environment  (ks  =  10 3  N/m),  the  closed- 
loop  poles  do  not  move  to  the  right  half  plane  for  the  same  force  gains  as  shown 
in  Figure  5.6.  These  two  cases  agree  with  the  behavior  predicted  by  the  general 
analysis  of  the  previous  section. 

5.2.3  Experimental  Verification  of  Instability 

In  this  section,  experimental  results  on  the  third  link  of  the  direct  drive  arm  are 
presented  to  verify  the  analyses  of  previous  sections.  The  third  link  is  being  con¬ 
trolled  in  a  pure  force  control  mode  as  shown  on  the  block  diagram  of  Figure  5.7. 
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Figure  5.7:  Simple  Force  Controller  with  K/  =  0.222 


The  Barry  Wright  FS6-120A  6-axis  force/torque  sensor  was  used  to  measure  the 
contact  force.  As  the  block  diagram  shows,  the  tip  force  sensor  feedback  is  a  pure 
gain  and  has  no  dynamic  compensation.  For  the  results  shown  in  this  section  I  did 
not  make  any  attempt  to  reduce  the  instabilities  that  were  present.  Such  efforts  are 
discussed  in  the  Sections  5.3,  5.4,  and  5.5. 

Figure  5.8  shows  the  step  responses  of  the  simple  force  controller  on  three  differ¬ 
ent  surfaces  using  same  gains  and  inputs.  The  negative  bias  shown  on  the  top  plot 
is  from  an  offset  drift  in  the  force  oensor  and  should  be  ignored.  As  expected,  the 
robot  becomes  unstable  when  it  comes  in  contact  with  the  stiff  aluminum  surface. 
The  spikes  on  the  force  data  are  produced  as  the  robot  bounces  on  this  surface. 
The  stability  is  improved  when  it  is  in  contact  with  a  rubber  surface.  Since  the 
rubber  used  in  this  experiment  is  a  hard  rubber,  it  is  still  quite  underdamped.  But 
as  the  root  locus  (Fig.  5.6)  of  the  section  5.2.2  indicated,  the  closed  loop  poles  are 
still  on  the  left  hand  plane,  and  the  robot  is  stable  and  remains  in  contact  with  the 
surface.  The  last  plot  of  Figure  5.8  is  the  force  step  response  of  the  robot  in  contact 
with  my  fingers.  Since  my  fingers  (and  the  hand)  are  very  compliant,  the  robot  is 
stable  under  the  simple  force  control. 

This  and  the  last  sections  have  demonstrated  some  of  the  dynamic  stability 
problems  associated  with  a  force-controlled  system  present  even  in  a  single  joint 
system.  In  the  next  three  sections,  I  will  present  several  methods  of  remedying 
those  stability  problems. 


ALUMINUM  SURFACE 


FINGER  SURFACE 


Figure  5.8:  Step  responses  on  three  surfaces 


5.3  Compliant  Coverings 


As  shown  in  equations  (5.3)  and  (5.7),  the  force  sensor  feedback  is  essentially  a 
position  feedback  with  the  gain  dominated  by  the  stiffness  ks,  representing  the 
effective  stiffness  of  the  force  sensor  and  the  environment.  In  general,  the  stiffness 
of  a  wrist  force  sensor,  kt ,  is  very  high  so  that  it  has  a  good  force  resolution  and 
dynamic  range.  Since  the  sensor  mass  is  typically  small,  the  effective  stiffness  k^ 
is  a  result  of  serial  combination  of  the  sensor  stiffness  k,  and  the  environment 
stiffness  ktnv.  Then,  to  reduce  ks,  one  can  either  make  the  sensor  soft  or  make  the 
environment  appear  soft  by  attaching  a  compliant  covering  to  the  contact  surface. 
Either  approach  gives  the  same  result  and  the  root  locus  of  such  a  system  will  have 
the  pattern  shown  in  Figure  5.6  of  Section  5.2.2.  The  experimental  result  for  such 
a  system  can  be  seen  in  Figure  5.8  in  the  force  step  response  on  rubber  or  finger 
surface. 

Whitney  (1985)  and  Roberts  (1985)  have  both  suggested  using  soft  sensors.  But 
that  may  not  be  very  practical  since  the  softer  the  sensor  is,  the  more  the  sensor 
will  bend.  Then  the  dynamic  range  of  the  sensor  will  be  limited  and  it  would  also 
be  difficult  to  control  the  tip  position  accurately  due  to  large  sensor  deflections 
(Roberts,  1985).  On  the  other  hand,  if  a  thin  compliant  covering  is  used  on  top  of 
the  force  sensor,  the  environment  will  always  seem  soft  to  the  force  controller.  This 
method  preserves  the  large  dynamic  range  of  a  stiff  force  sensor  and  also  improves 
stability. 

Using  compliant  coverings  is  not  unlike  how  human  arms  are  made.  In  fact,  the 
skin  on  our  arm  helps  us  greatly  in  force  control.  It  absorbs  the  energy  during  the 
collision  phase  of  a  contact  with  a  stiff  environment  and  it  also  acts  as  a  contact 
sensor  to  give  us  some  relative  position  information.  In  that  respect,  it  would  be 
even  better  to  make  the  compliant  coverings  to  be  the  tactile  sensors  of  the  type 
discussed  in  (Siegel,  Garabieta,  and  Hollerbach,  1986). 


5.4  Adaptation  to  the  Environment  Stiffness 

Another  method  in  dealing  with  a  stiff  environment  is  to  somehow  estimate  the 
environment  stiffness  and  then  to  incorporate  the  knowledge  of  the  environment 
in  the  controller  to  achieve  a  stable  and  robust  behavior.  One  simple  method  of 


Figure  5.9:  Fourth  order  model  of  the  robot  and  the  environment 


incorporating  such  knowledge  is  to  vary  the  velocity  gains  according  to  the  environ¬ 
ment  stiffness  and  damping,  so  that  the  system  remains  stable  without  being  too 
sluggish.  The  problem,  then,  is  first  to  identify  the  dynamic  characteristics  of  the 
environment.  Here  I  will  present  two  methods,  one  simpler  than  the  other. 

Let’s  consider  a  fourth  order  model  for  a  situation  where  a  manipulator  is  in 
contact  with  its  environment  (Figure  5.9).  The  force  sensor  is  modelled  as  a  spring 
of  stiffness  k,.  The  environment  is  modelled  as  a  spring,  a  damper,  and  a  mass. 
The  mass  mE  is  the  manipulator  load,  a  tool,  for  example.  In  practical  situations, 
if  the  system  becomes  unstable,  the  manipulator  will  lose  contact  with  the  environ¬ 
ment.  In  this  section,  however,  I  am  considering  a  simplified  situation  where  the 
manipulator  remains  in  contact.  The  former  situation  is  more  complex  and  will  not 
be  addressed  in  this  thesis. 

The  behavior  of  the  system  in  Figure  5.9  is  described  by, 
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and  x,  is  the  displacement  of  the  sensor  spring.  Ignoring  the  viscous  damping  in 
the  sensor,  the  equation  for  the  dynamics  of  the  environment  only  is  decoupled  as, 


/,  —  k,  x,  —  mExE  +  1>exe  +  kExE 


(5.11) 
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The  measurement  of  f,  is  available  using  a  wrist  force/torque  sensor.  Although  x E 
cannot  be  measured  directly,  it  is  related  by, 


Xrobot  -xt  =  xE  (5.12) 

where  xroi0t  is  the  movement  of  the  manipulator  from  the  point  of  contact  with  the 
environment  and  x ,  =  f,/kt.  However,  typically  we  will  not  be  able  to  meat  ure  the 
other  state,  xE. 

5.4.1  Least  Squares 

One  method  of  identifying  the  dynamics  of  the  environment  is  to  use  a  least  squares 
algorithm.  If  the  measurements  of  xE  and  xE  as  well  as  f,  and  xE  were  available, 
than  we  can  use  straightforward  least  squares  algorithm  on  the  continuous  time 
equation  (5.11).  However,  as  discussed  above,  at  best  we  only  have  measurements 
of  f,  and  xE.  In  this  situation,  the  continuous  time  equation  for  the  dynamics  of 
the  environment  can  be  transformed  to  a  discrete  version  by  a  number  of  different 
methods  (Franklin  and  Powell,  1980).  Then,  using  least  squares  method,  the  co¬ 
efficients  for  the  discrete  equation  can  be  estimated.  The  actual  continuous  time 
parameters  can  then  be  obtained  from  these  discrete  coefficients  by  simple  alge¬ 
braic  transformations.  In  the  rest  of  this  section,  these  steps  are  derived,  and  both 
simulation  and  experimental  results  are  presented. 


Derivations 

The  Laplace  transform  of  the  continuous  time  equation  (5.11)  for  the  dynamics  of 
the  environment  is 


f,(s)  =  k,x„(s )  =  ( mEs 2  +  bEs  +  kE)xE(s).  (5.13) 


For  the  bilinear  transformation  method  of  converting  a  continuous  time  system  to 
the  discrete  time  domain,  the  relation  between  s  and  z  is  given  by 
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(5.14) 


Substituting  (5.14)  into  (5.13), 


The  difference  equation  corresponding  to  (5.15)  is 

ft[n]  +  2 ft[n  -  1]  +  f,\n  -  2]  =  a0x,[n]  +  aiXt[n  -  l]  +  a2x,[n  -  2).  (5.16) 

Then,  using  a  well  documented  recursive  least  squares  method  (Ljung,  1983),  the 
coefficients  ao,  ai,  and  a2  can  be  estimated.  The  continuous  time  parameters,  then, 
are  computed  from  the  estimated  coefficients  by  the  following  algebraic  relations: 

T 

t>E  =  -r(ao  —  ai) 

4 


mB  =  — (ao  +  aa  -  oi) 

1  8mjs. 

kE  =  j  (fli  +  -fr) 


(5.17) 


Since  the  bilinear  transformation  is  not  an  exact  transformation,  the  above  deriva¬ 
tion  is  also  approximate.  But  for  fast  sampling  time,  the  transformation  is  quite 
accurate. 

Simulation 

The  simulation  results  of  the  recursive  least  squares  estimation  is  shown  in  Figure 
5.10.  The  signal  is  a  step  response  with  some  random  noise  and  the  sampling 
frequency  is  200  Hz.  As  shown  in  the  figure,  the  stiffness  estimate  reaches  the  actual 
value  quickly,  within  0.05  second  or  10  samples.  The  damping  estimate  reaches  the 
final  value  in  0.1  second.  The  estimate  for  the  mass  settles  to  the  final  value  after 
0.2  second,  and  there  is  a  significant  bias  error  in  this  estimate.  These  behaviors  are 
expected.  The  bB  and  mB  parameters  are  mainly  affected  by  the  xB  and  xB  signals 
respectively,  whose  measurements  are  not  available  directly.  Therefore,  the  discrete 
algorithm  is  essentially  numerically  differentiating  and  double  differentiating  the  xB 
measurements  in  order  to  arrive  at  estimates  of  6^  and  mB.  The  estimates  for  these 
parameters  then  should  not  be  as  good  as  the  estimate  of  kB.  For  stability,  the 
stiffness  parameter  is  the  most  important  and  as  shown  in  the  simulation  results, 
it  may  be  possible  to  estimate  it  quickly  enough  for  the  controller  to  adapt  to  the 
changing  environment  conditions. 

Experiments 
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Figure  5.11:  Experimental  Stiffness  Estimate  on  Aluminum  Surface 


Estimation  using  the  least  squares  method  was  tested  on  two  different  surfaces, 
aluminum  and  rubber.  Figures  5.11  and  5.12  show  the  step  force  responses  for  one 
link  robot,  the  position  displacements,  and  the  estimation  results  of  the  effective 
environment  stiffness  of  an  aluminum  and  a  hard  rubber  surface,  respectively.  The 
measurements  of  the  position  displacements  were  from  the  resolver  at  the  joint. 
Since  the  stiffness  of  the  sensor  is  very  high  (>>  10 6N/m),  I  neglected  the  deflec¬ 
tions  of  the  force  sensor  in  the  position  measurements. 

The  results  show  that  the  estimated  value  of  stiffness  for  the  hard  rubber  surface 
is  60%  of  that  for  the  aluminum  surface.  Although  the  actual  stiffness  of  aluminum 
may  be  much  higher  than  the  estimate,  the  effective  stiffness  of  the  aluminum  sur¬ 
face  and  the  end  effector  (a  bearing  attached  to  sensor)  together  is  lower  than  the 
actual  stiffness.  In  control,  it  is  this  effective  stiffness  that  is  important  for  stability 
considerations.  The  estimates  for  the  mass  and  the  damping  were  not  consistent 
and  are  not  shown.  As  discussed  in  the  simulation  section,  this  was  expected  since 
those  estimates  depend  on  the  derivatives  of  the  position  data.  For  the  examples 
presented,  the  positional  displacements  are  too  small  for  the  derivatives  to  be  ac¬ 
curate.  With  the  16  bit  resolution  resolver,  the  maximum  displacements  are  only 
2.5  bits  for  aluminum  and  3.5  bits  for  rubber. 


5.4.2  Adaptive  Observer 

For  the  environment  equation  (5.11),  an  adaptive  observer  technique  (Narendra 
and  Kudva,  1974;  Narendra  and  Valavani,  1976;  Shih,  1985a)  may  also  be  used 
to  estimate  the  mass  mE,  the  stiffness  kE,  and  the  damping  constant  bE  of  the 
environment.  Actually  mE  may  already  be  known  through  load  identification,  but 
I  will  continue  to  treat  it  as  an  unknown  in  the  following  analysis.  Following  the 
method  presented  by  Narendra  and  Kudva  (1974),  a  stable  adaptive  observer  can 
be  formulated  for  the  equation  (5.11).  Dividing  by  mE ,  (5.11)  can  be  rewritten  as, 


Dft  =  xE  +  (3xE  +  ClxE 


(5.18) 


where 


D  =  -L,  0  =  is.,  n  = 

mE  mE  mE 


Define  additional  state  variables  y  and  w  as, 


w  =  —Aid  +  ft 


(5.19) 


In  terms  of  the  new  state  variables,  (5.18)  can  be  written  as, 
xE  =  -0xE  -  Cly  +  Dw  +  \xE  +  A(/?  -  A )y 


(5.20) 


The  derivation  of  (5.20)  is  in  Appendix  3.  Equations  (5.19)  and  (5.20)  together 
represent  a  non-minimal  realization  of  the  original  equation  (5.18).  In  a  matrix 
form,  this  equivalent  non-minimal  realization  can  be  written  as, 
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(5.21) 


All  of  the  states  in  (5.21)  can  be  either  measured  or  computed  by  (5.19).  Then, 
following  the  structure  of  (5.20),  the  adaptive  observer  equations  are  formulated. 

xE  =  -0xE  -  Cly  +  Dw  +  XxE  +  -  A)y  -  a{xE  -  xE) 


(5.22) 


±D  =  -7s  erv 


This  adaptive  observer  can  be  proved  to  be  stable  using  Lyapunov  analysis,  given 
sufficiently  rich  inputs.  There  exists  a  discrete  version  of  the  above  formulation 
(Kudva  and  Narendra,  1974)  and  also  other  forms  of  adaptive  observer  that  may 
be  useful  for  this  application  (Shih  and  Lang,  1985a,  1985b). 

There  may  be  other  methods  for  identifying  the  environment  dynamics.  One 
possible  method  is  to  estimate  the  environment  stiffness  kE  by  estimating  the  fre¬ 
quency  of  oscillation  caused  by  interaction  with  the  environment.  The  frequency 
information  may  be  obtained  by  taking  the  FFT  of  the  force  measurement  f,. 

5.4.3  Feasibility 

The  above  methods  show  that  it  is  theoretically  possible  to  identify  the  dynamics 
of  the  environment  in  order  to  use  them  in  the  control  law.  However,  as  shown 
in  the  experimental  estimation  results,  using  practical  sensors,  we  may  not  have 
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enough  joint  position  sensor  resolution  to  measure  xe  accurately.  Assuming  that 
the  force  sensor  is  much  stiffer  than  the  environment,  for  a  16  bit  resolution  joint 
angle  measurements  and  0.5  m  lever  arm,  a  static  analysis  (/  =  JceXe)  shows  the 
following  upper  limits  in  the  stiffness  of  an  environment  that  can  be  estimated: 

force  ks 

IN  2  x  104  N/m 

10 N  2  x  105  N/m 

This  shows  that  even  for  rather  optimistic  16  bit  resolution  measurements,  unless  the 
contact  force  is  undesirably  large,  stiff  environments  cannot  be  identified  accurately. 
This  characteristic  may  limit  the  usefulness  of  this  approach  in  dealing  with  stiff 
environments. 

5.5  Joint  Torque  Control 

I  have  so  far  assumed  that  a  force-controlled  system  uses  a  high  gain  feedback  of 
the  signal  from  a  tip  force  sensor,  which  i3  typically  characterized  as  a  very  stiff 
spring.  From  the  stability  point  of  view,  feedback  control  using  the  output  of  such 
a  force  sensor  is  equivalent  to  a  very  large  gain  position  feedback  which  can  lead 
to  instability.  Stability  can  be  improved  if  the  loop  gain  is  reduced.  Adding  a 
compliant  covering,  as  discussed  in  Section  5.3,  was  one  such  approach.  Lowering 
the  force  feedback  gain  also  reduces  the  loop  gain  but  also  has  the  undesirable  effect 
of  deteriorating  the  force  resolution.  Another  useful  approach  is  to  achieve  force 
control  without  using  the  tip  force  sensor  in  the  feedback  loop,  hence  removing  the 
high  gain  component  from  the  closed  loop  system.  Instead,  the  end  effector  force 
can  be  controlled  by  relying  on  the  measurements  of  and  the  ability  to  command 
joint  torques  accurately.  The  loop  gain  is  0  since  such  a  control  method  is  an 
open-loop  control  from  the  point  of  view  of  interaction  forces  at  the  end  effector. 
However,  in  practical  systems,  a  closed-loop  torque  servo  is  implemented  at  each 
joint  so  that  the  joint  torque  can  be  specified  accurately. 

This  approach  has  been  investigated  previously  by  Wu  and  Paul  (1980)  and 
also  by  Luh,  Fisher,  and  Paul  (1983)  using  strain  gauges  on  the  motor  shaft  of  a 
geared  system,  but  they  did  not  use  their  experimental  joint  torque  control  method 
to  perform  any  active  force  control.  Instead,  the  joint  torque  feedback  was  used 


mainly  to  reduce  frictional  effects  at  the  joints  for  the  Stanford  Arm  (Luh,  Fisher, 
and  Paul,  1983).  For  the  direct  drive  arm,  since  there  is  very  little  friction  at  the 
joints,  the  measurements  of  joint  torques  can  be  obtained  by  measuring  the  motor 
currents.  In  Chapter  3, 1  used  this  capability  to  compute  joint  torques  in  order  to 
estimate  the  link  inertial  parameters. 

Wu  and  Paul  (1980)  presented  a  good  comparison  between  using  wrist  force  sens¬ 
ing  and  joint  torque  sensing  in  implementing  force  control.  Wrist  sensing  provides 
accurate  force/torque  measurements  at  the  hand;  but  because  the  robot  structure 
is  inherently  a  low  bandwidth  flexible  system  and  the  sensor  is  situated  at  the  end 
of  this  structure,  a  high  gain  feedback  will  produce  instability  as  shown  in  Section 
5.2.  Therefore,  only  a  slow  closed  loop  system  can  be  implemented  stably  using 
a  wrist  sensor.  On  the  other  hand,  since  joint  torque  sensors  are  situated  before 
the  low  bandwidth  robot  structure,  a  high  bandwidth  torque  inner  loop  can  be  im¬ 
plemented  around  each  joint.  But  since  the  sensors  are  not  at  the  hand,  the  hand 
forces  and  torques  cannot  be  inferred  as  accurately  as  the  wrist  sensing. 

Since  both  the  wrist  sensing  and  the  joint  sensing  have  good  and  bad  features, 
one  reasonable  method  is  to  combine  the  two  methods  to  provide  a  stable  high 
bandwidth  force-controlled  system.  The  high  bandwidth  open-loop  joint  torque 
control  with  inner  torque  servo  loop  will  provide  stability  and  fast  response,  and  the 
lower  bandwidth  outer  loop  with  wrist  force  sensing  will  provide  extra  accuracy.  To 
the  best  of  my  knowledge,  this  method  has  not  been  implemented  before,  probably 
due  to  the  lack  of  a  suitable  manipulator.  In  the  direct  drive  arm,  one  has  this 
capability  since  torques  can  be  commanded  quite  accurately  at  each  joint. 

In  the  rest  of  this  section,  the  stable  property  of  an  open-loop  force  controller 
using  joint  torque  sensing,  is  discussed  in  more  detail,  and  the  results  of  one-link 
force  control  experiments  are  presented  demonstrating  the  performances  of  using 
joint  torque  sensing  and  also  the  combination  of  joint  torque  and  wrist  force  sensing. 


5.5.1  Dominant  Pole 

Without  the  wrist  sensor  in  the  force  feedback  loop,  the  dynamics  of  a  simple 
manipulator  in  contact  with  its  environment  is  given  by 

/  —  fcgx  =  mx  +  bx  (5.23) 
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If  this  system  is  controlled  purely  by  commanding  the  force  or  the  torque  at  the 
joint,  i.e.  in  open  loop  mode,  the  response  should  be  very  underdamped  since  ks 
may  be  high  for  a  stiff  environment.  One  simple  classical  compensation  method 
of  improving  stability  is  to  create  a  dominant  pole  in  the  loop  transfer  function 
(Roberge,  1975).  This  can  be  done  simply  by  putting  a  low-pass  filter  in  the  forward 
path  so  that 

r  =  (5.24) 

s  +  a 

where  r  is  the  actual  input  torque  (or  currents)  to  the  actuator  and  a  is  much 
less  than  the  resonant  frequency  of  the  original  system.  Then,  the  total  system 
dynamics  is  described  by 

x(a)  =f(  — (  — _ — \ )  .  (5,25) 

v  \s  +  a/  \ms2  +  bs  +  key  v 

Because  of  the  dominant  pole  at  s  =  a,  this  compensated  system  behaves  in  a  much 
more  stable  manner  despite  the  two  high  frequency  underdamped  poles.  Figure 
5.13  compares  the  step  responses  of  the  above  system  (5.25)  to  the  original  system 
without  the  dominant  pole  (5.23). 

For  real  actuators,  the  explicit  low-pass  filtering  of  the  input  signal  may  not 
even  be  necessary,  since  the  combined  system  of  the  amplifiers  and  the  manipulator 
structure  behaves  essentially  as  a  low-pass  filte:.  For  the  MIT  Serial  Link  Direct 
Drive  Arm,  there  is  a  an  analog  current  loop  at  each  amplifier  to  the  motor  which, 
together  with  the  rotor  inertia,  has  the  cutoff  frequency  at  approximately  30  Hz. 
Because  of  this  low-pass  characteristic,  explicit  low-pass  filtering  was  not  necessary 
to  achieve  stability.  However,  even  for  the  direct  drive  motors,  there  are  some 
nonlinear  characteristics  as  deadzone  for  small  torques  (Asada,  Youcef-Toumi,  and 
Lim,  1984),  cogging,  and  other  effects  due  to  the  imperfect  commutation  circuitry. 
Some  of  these  nonlinear  characteristics  can  be  reduced  by  implementing  another 
torque  feedback  loop  at  each  joint.  As  discussed  in  Chapter  3,  for  the  DDARM 
the  torque  at  each  joint  is  computed  from  the  three  phase  currents  by  the  following 
relation, 

t  =  Kr(Ia  sin(n0  -f-  of  f set)  -f  7j  sin(n0  +  120  4-  offset)  -I-  Jc  sin (n0  +  240  +  of  f set)) 


where 


160°  for  Joint  1 
—33.3  for  Joint  2 
—108  for  Joint  3 

Figure  5.14  shows  the  block  diagram  of  the  joint  torque  servo  and  also  the  com¬ 
parison  between  the  commanded  and  the  measured  torques  for  the  third  joint  of 
the  DDARM  with  and  without  the  added  joint  torque  feedback.  The  pole  of  the 
lag  filter  shown  in  the  figure  is  separate  from  the  dominant  pole  discussed  above. 
The  dominant  pole  refers  to  the  low  pass  structure  of  the  whole  actuator  system, 
including  the  torque  servo,  amplifier,  and  the  motor.  As  the  plots  in  Figure  5.14 
show,  torques  can  be  commanded  accurately  at  each  joint. 

The  analysis  of  this  section  shows  that  the  force  control  via  joint  torque  sensing 
is  stable  and  well  behaved,  due  to  the  open-loop  structure  and  the  dominant  pole 
created  either  explicitly  or  inherently  by  the  amplifier  and  the  robot  structure.  But 
as  discussed  earlier,  it  is  also  desirable  to  include  the  wrist  force  sensing  to  improve 
the  accuracy  of  the  force  control  system.  In  such  a  case,  using  the  same  analysis  as 
above,  one  should  also  use  a  low  pass  filter  in  the  control  loop  involving  the  wrist 
force  sensor  feedback,  thus  creating  a  dominant  pole.  The  pole  of  this  wrist  sensing 
loop  should  be  at  a  much  lower  frequency  than  the  pole  of  the  joint  torque  path,  so 
that  the  stability  is  not  affected.  Then,  the  combined  multi-feedback  loop  system 
should  have  the  stability  and  the  high  bandwidth  from  the  joint  torque  control 
mechanism,  and  the  steady  state  accuracy  from  the  wrist  iorce  control  mechanism. 
The  block  diagram  of  a  such  a  system  is  shown  in  Figure  5.15. 


f  15  for  Joint  1 
|  9  for  Joints  2 


5.5.2  One  Link  Force  Control  Experiments 

The  two  suggested  stable  force  control  methods  were  implemented  on  the  third 
link  of  the  DDARM.  The  first  method,  using  joint  torque  sensing  only,  is  an  open 
loop  method  of  commanding  the  appropriate  torque  at  the  joint  and  relying  on  the 
dominant  pole  provided  by  the  joint  torque  servo  and  the  amplifier  to  keep  the 
system  stable  in  contact  with  stiff  environments.  In  this  case  the  wrist  force  sensor 
is  used  only  to  record  the  force  data  at  the  tip  of  the  robot  and  is  not  used  in  the 

‘The  offsets  are  due  to  abnormalities  in  the  commutation  circuitry  as  well  as  offsets  in  resolvers. 
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Figure  5.15:  Structure  of  a  general  force  controller 

feedback.  The  second  method  uses  both  the  joint  torque  sensing  and  the  wrist  force 
sensing  as  shown  in  Figure  5.15.  The  two  methods  are  summarized  below. 

1.  Using  joint  torque  sensing  only: 

T  =  fdeiired  ’  *3,  k  =  0.4445  Ul 

2.  Combination  of  joint  torque  sensing  and  wrist  force  sensing: 


T  —  fdeiired  '  ^3  d"  kf 


( fdeiired  f measured) 


The  third  link  has  an  explicit  joint  torque  feedback  loop  shown  in  Figure  5.14  im¬ 
plemented  digitally  at  500  Hz.  The  sampling  frequency  of  500  Hz  was  used  through¬ 
out  the  one  link  experiments. 


Force  Step  Response 

In  Figure  5.16,  a  stable  force  step  response  of  the  second  method  is  compared  to  the 
unstable  response  of  a  simple  pure  gain  force  feedback  without  the  dominant  pole 
for  the  robot  in  contact  with  a  stiff  aluminum  surface.  The  negative  bias  shown  on 
the  top  plot  is  from  an  offset  drift  in  the  force  sensor  and  should  be  ignored.  The 
plots  are  untouched  without  any  low-pas3  filtering  of  the  data  to  reduce  the  noise 
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for  plotting  purposes.  As  the  figure  shows,  although  there  is  a  significant  amount 
of  noise  in  the  force  data,  the  step  response  of  the  compensated  system  is  definitely 
stable,  whereas  the  response  of  the  system  without  the  proper  compensation  is 
unstable  and  the  manipulator  bounces  on  the  environment  surface. 

Figure  5.17  shows  the  force  step  responses  of  the  two  stable  methods  discussed 
above.  As  expected  from  the  analysis,  there  is  no  noticeable  difference  in  dynamic 
behavior  between  the  two  controllers;  but  the  second  method,  combining  the  joint 
torque  sensing  and  the  wrist  force  sensing,  has  much  better  steady  state  accuracy. 
For  either  method,  the  accuracy  is  worse  for  the  lower  force  inputs  since  the  nonlin¬ 
ear  characteristics  (sue!'  as  dead  zone  .and  cogging)  of  the  motor  and  the  amplifier 
for  small  torque  leve’ ,  are  more  severe  (Asada,  Youcef-Toumi,  and  Lim,  1985).  The 
commanded  force  levels  (10  N  and  15  N)  used  in  the  experiments  are  less  than  2% 
of  the  capacity  of  the  DDARM,  which  can  exert  greater  than  500  N  of  force  at  its 
tip. 


Performance  Tests 


Step  inputs  are  not  really  the  best  inputs  in  testing  the  performance  of  force  con¬ 
trol  methods.  Their  high  frequency  contents  and  the  large  initial  torques  that  are 
required  at  the  discontinuities  can  excite  undesirable  structural  modes.  But  it  does 
qualitatively  test  stability.  For  evaluating  performance  of  force  controllers,  better 
methods  are: 


1.  Sine  wave  force  command  with  the  manipulator  in  contact  with  the  stationary 
environment, 


2.  Constant  force  command  with  the  manipulator  in  contact  with  the  environ¬ 
ment  moving  in  a  sinusoid  trajectory. 


Both  of  these  test  inputs  may  give  us  information  about  bandwidth. 


Sine  Wave  Force  Command  The  responses  to  a  1  Hz  sine  wave  force  com¬ 
mand  are  given  in  Figure  5.18  for  the  two  stable  methods.  Similar  to  the  step 
response  results,  the  accuracy  for  the  second  method  is  much  greater  than  the  first 
method.  But  again,  for  either  method,  neglecting  the  bias  errors,  the  manipulator 
follows  the  sine  wave  command  faithfully.  Figure  5.19  shows  the  responses  of  the 


mm m 


!  ALUMINUM:  WITHOUT  DOMINANT  POLE 


Figure  5.16:  Force  step  responses  with  and  without  the  dominant  pole 
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FORCE  and  TORQUE  FEEDBACK:  POLE  at  1  Hz 


Figure  5.17:  Force  step  responses  for  the  two  stable  methods  overlayed  on  the  step 
inputs 
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combined  method  to  sine  wave  force  commands  of  frequencies  up  to  20 Hz.  Al¬ 
though  there  is  a  noticeable  lag  in  the  measured  data  for  higher  frequencies,  the 
manipulator  still  has  no  trouble  followi:  g  the  sine  wave  command.  Therefore,  us¬ 
ing  the  usual  definition  of  bandwidth  (frequency  at  which  the  output  magnitude  is 
reduced  by  3dB  from  the  DC  value),  the  bandwidth  tested  in  this  way  is  greater 
than  20 Hz. 


Constant  force  command  with  positional  disturbances  The  second  per¬ 
formance  test,  a  constant  force  command  while  the  contacting  environment  is  mov¬ 
ing,  is  made  with  an  eccentric  cam  on  a  gear  motor  as  shown  in  Figure  5.20.  The 
circular  cam  is  attached  to  the  gear  motor  at  a  point  0.5  inch  off  from  the  center 
providing  an  approximate  sine  wave  positional  disturbance  to  the  force  controller. 
The  responses  of  the  two  methods  are  plotted  on  Figure  5.21.  The  second  method 
is  able  to  follow  the  cam  accurately  with  the  desired  force.  Neglecting  the  high 
frequency  noise,  the  low  frequency  variation  in  the  measured  force  is  within  10%  of 
the  desired  12.5 N.  Higher  cam  speed  resulted  in  larger  variations. 

It  is  difficult  to  define  a  measure  of  bandwidth  for  this  test  because  this  test  is 
really  a  disturbance  rejection  test.  The  movements  of  the  cam  provided  a  positional 
disturbance  to  the  force-controlled  system  as  shown  in  the  simplified  block  diagram 
of  Figure  5.22.  Therefore,  a  reasonable  measure  of  this  disturbance  rejection  is  the 
frequency  wir  at  which  the  variation  is  10%  (5.27). 


f command  ft 

i command 


=  10% 


(5.27) 


Under  this  measure,  this  system  is  well  behaved  up  to  approximately  0.8 Hz.  The 
response  of  the  first  method  is  not  as  accurate  at  this  cam  speed,  suggesting  a 
slightly  lower  frequency  limit. 

The  frequency  limit  determined  by  the  positional  disturbance  is  much  lower 
than  the  bandwidth  determined  by  the  sinusoid  force  trajectory  testing.  This  is 
not  surprising  since  for  the  sinusoid  trajectory  tests,  the  movement  of  the  link  is 
infinitesimal;  whereas  for  the  cam  tests,  the  controller  has  to  move  the  large  link 
inertia  over  a  significant  distance.  It  is  not,  however,  clear  which  measurement 
of  performance  is  more  useful.  For  applications  that  involve  following  undulated 
surfaces,  the  positional  disturbance  test  is  more  relevant.  But  for  peg-in-hole  types 
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Figure  5.21:  Force  responses  to  a  sinusoidal  positional  disturbance 
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disturbance 


Figure  5.22:  Positional  Disturbance  Rejection 

of  applications  in  which  the  movements  are  very  small,  the  sinusoid  trajectory  test 
may  be  more  relevant. 


Chapter  6 


Kinematic  Stability  Issues  in 
Force  Control 


6.1  Introduction 

The  dynamic  stability  problems  discussed  in  Chapter  5  can  occur  whether  the  ma¬ 
nipulator  consists  of  simply  one  link  or  multi-links.  For  a  multi-link  manipulator, 
there  is  typically  some  type  of  coordinate  transformation  either  at  the  planning 
stage  or  in  the  active  feedback  computation  path.  If  the  transformation  is  in  the 
feedback  path,  then  it  will  have  some  effect  on  the  dynamics  of  the  closed-loop  sys¬ 
tem.  In  the  worst  case,  the  kinematic  transformation  in  the  feedback  path  may  even 
make  the  system  unstable  by  making  the  closed-loop  poles  move  into  the  right  half 
.s-plane.  This  chapter  focuses  on  these  issues  of  kinematically  induced  instability. 
The  purpose  is  not  to  prove  any  type  of  stability  but  instead  to  prove  instabilities 
by  counter  examples  against  stability. 

The  simplest  coordinate  system  to  use  in  controlling  a  manipulator  is  the  joint 
coordinate  system.  A  stable  and  well  behaved  response  can  be  obtained  using  even 
very  simple  control  algorithms  in  this  way.  But  the  simplest  coordinate  system  for 
humans  to  visualize  and  plan  tasks  for  a  manipulator  is  the  cartesian  coordinate 
system.  There  are  several  ways  to  control  a  non-cartesian  robot  as  a  polar  manip¬ 
ulator  or  a  revolute  manipulator  in  cartesian  coordinates.  In  pure  position  control 
mode,  the  most  widely  used  method  is  to  compute  the  inverse  kinematics  at  the  tra- 
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jectory  planning  stage  and  then  use  joint  coordinates  for  the  real  time  control.  For 
compliance  or  force  control,  the  algorithms  must  deal  with  the  interaction  forces 
as  well  as  the  desired  positions  and  cannot  simply  execute  the  preplanned  joint 
trajectory.  As  stated  in  the  earlier  chapters,  there  are  several  well  known  carte¬ 
sian  compliance  control  algorithms.  They  are  summarized  below,  and  their  main 
structures  are  shown  on  Figure  6.1. 

Hybrid  Control  (Raibert  and  Craig,  1981)  r1  The  cartesian  positions  and  the 
velocities  are  computed  from  the  joint  positions  and  velocities,  respectively, 
by  direct  or  forward  kinematics.  Neglecting  the  integral  terms, 

t  =  K„J _1S(xrf  -  x)  +  K„J_1S(x^  -  x)  +  K,Jr(I  -  S)(fd  -  f)  (6.1) 

where  S  is  the  diagonal  selection  matrix  whose  (t ,  *)  entry  is  1  if  ttfcaxis  is  to 
be  position  controlled,  and  0  if  it  is  to  be  force  controlled. 

Resolved  Acceleration  (Lull,  Walker,  and  Paul,  1980b;  Khatib,  1983):  In  the 
original  paper  by  Luh,  Walker,  and  Paul,  the  resolved  acceleration  method 
was  formulated  only  as  a  dynamic  cartesian  trajectory  controller.  But  with  a 
simple  modification,  this  method  can  be  used  to  control  force  and  position  for 
different  cartesian  degrees  of  freedom  as  the  hybrid  controller.  The  modified 
resolved  acceleration  controller  is  given  by  (6.2) . 

t  =  M(q)J_1(q)[Sx^  -  h(q,  q)]  +  b(q,q)  +  g(q)  +  JT(I  -  S)f*  (6.2) 

x^  =  xd  +  K^Xj  -  x)  +  KpJxj  -  x),  h(q,q)  =  j(q)q 

f*  is  the  command  vector  for  active  force  control,  which  is  the  only  modifi¬ 
cation  from  the  original  formulation  by  Luh,  Walker,  and  Paul.  As  shown  in 
Appendix  4,  Khatib’s  operational  space  method  is  essentially  identical  to  this 
modified  resolved  acceleration  controller.  The  impedance  control  algorithm  in 
(Hogan,  1985b)  also  fits  under  the  category  of  resolved  acceleration  method. 
The  only  difference  is  that  the  force  gain  is  a  function  of  the  desired  mass  for 
the  impedance  controller. 

Actually  all  three  of  the  methods  discussed  are  hybrid  controllers  in  that  all  of  them  can  be 
formulated  to  control  position  and  force  for  different  axes.  However,  for  convenience,  when  the  term 
the  hybrid  control  is  mentioned  without  any  other  clarifying  description,  I  am  always  referring  to 
the  method  of  Raibert  and  Craig. 
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(a)  Hybrid  Controller 


(b)  Stiffness  Controller 
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(c)  Resolved  Acceleration  Controller 


Figure  6.1:  Block  diagrams  for  the  hybrid,  the  stiffness,  and  the  resolved  accelera¬ 
tion  controllers  shown  without  the  velocity  terms 
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As  in  the  Raibert  and  Craig’s  method,  the  cartesian  positions  and  the  ve¬ 
locities  are  computed  from  the  joint  positions  and  velocities,  respectively,  by 
direct  or  forward  kinematics.  However,  this  controller  includes  the  complete 
rigid  body  dynamics  of  the  manipulator,  so  that  if  the  dynamic  modelling  were 
to  be  exact,  the  motion  of  the  manipulator  would  be  completely  decoupled 
at  the  end  effector  in  cartesian  coordinates.  Then,  the  response  under  the 
pure  position  control  mode  would  be  that  of  a  unit  mass  along  each  cartesian 
degree  of  freedom.  For  example,  the  resulting  behavior  in  the  x-axis  is 

xe  +  kvxxe  +  kpxxe  =  0,  where  xe  =  xd-  x.  (6.3) 

Stiffness  Control  (Salisbury,  1980):  The  desired  cartesian  trajectory  is  trans¬ 
formed  to  joint  coordinates  at  the  trajectory  planning  stage. 

r  =  JTKJ(q<j  -  q)  +  Kw(qd  -  q)  (6.4) 

Although  the  kinematic  errors  are  computed  in  joint  coordinates  in  Salisbury’s 
original  paper  (1980),  it  is  also  possible  to  implement  stiffness  control  by 
computing  the  errors  in  cartesian  coordinates  as  in  the  hybrid  control.  Then 
the  slightly  modified  stiffness  control  algorithm  is: 

r  =  JT(Kp(x<j  —  x)  +  K„(x<*  —  x))  (6.5) 

Since  the  stiffness,  not  the  pure  force,  is  to  be  controlled,  the  above  controller 
equations  are  shown  without  any  force  feedback  term.  A  force  term,  how¬ 
ever,  can  be  added  if  the  stiffness  matrix  alone  does  not  provide  enough  force 
resolution  or  if  pure  force  control  is  desired  in  some  direction. 

Potential  stability  problem  arises  since  the  coordinate  transformation,  either  by 
the  Jacobian  inverse  or  the  Jacobian  transpose,  is  directly  in  the  feedback  loop. 
Hence,  when  considering  the  closed-loop  stability  of  a  cartesian  force  controlled 
system,  the  effects  of  these  matrices  must  be  understood.  The  goal  of  this  chapter 
is  to  study  stability  problems  that  can  be  caused  by  kinematic  transformations  in 
the  feedback  loop.  In  particular,  it  will  be  shown  that  the  stiffness  and  the  re¬ 
solved  acceleration  methods  do  not  become  unstable  for  either  revolute  and  polar 
manipulators.  However,  due  to  its  inherent  control  structure,  the  hybrid  control 


method  proposed  by  Raibert  and  Craig  can  cause  instability  on  a  revolute  manip¬ 
ulator  regardless  of  the  choice  of  gains.  These  instabilities  occur  not  only  at  the 
points  of  kinematic  singularities,  where  the  Jacobian  inverses  are  not  defined,  but 
at  a  wide  range  of  the  manipulator  work  space,  where  the  Jacobian  inverses  are  well 
defined.  This  study  includes  both  intuitive  and  rigorous  analytical  results  as  well 
as  experimental  results  with  the  direct  drive  arm  to  support  the  above  claims.2 

A  planar  two-link  revolute  manipulator  (Figure  6.2)  will  be  mainly  used  in  the 
analyses. 


q  1  w  x 


Figure  6.2:  Simple  two-link  planar  manipulator 


For  this  planar  manipulator,  the  forward  kinematics  are 


llCi  +  /2C1J 
hsi  +  /jSn 


where  =  sin(0i)  and  sls  =  sin(0i  +  02).  (6.6) 


The  Jacobian  for  the  fixed  cartesian  coordinates  is 


~hsi  ~  fan  ~ 

x  =  39 


—  lisl  2 

•  • 

/jCu 


The  inverse  Jacobian  is 


0\  _  1  /jCu  /jSij  x  ^  ^ 

02  —liCi  —  l2Ci2  —llSi  —  l2Si2  y 

$  =  J"1* 

2The  motivation  for  this  chapter  came  from  the  author’s  observation  of  instability  when  the 
hybrid  controller  was  tried  on  the  DDARM. 
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For  the  lengths  /i  and  Zj, 1  am  using  the  lengths  of  the  DDARM  configured  as  a 
two-link  planar  manipulator  by  locking  the  second  joint  and  moving  only  the  first 
and  the  third  joints. 

li  =  0.462m,  Z2  =  0.445m 

6.2  Intuitive  Stability  Analysis  for  Revolute  Ma¬ 
nipulators 

Before  a  more  rigorous  treatment,  it  may  be  helpful  to  get  an  intuitive  understand¬ 
ing  of  why  and  how  the  instability  occurs.  The  analysis  presented  in  this  section 
is  approximate  and  not  mathematically  rigorous.  This  analysis  will  reveal  that 
there  indeed  is  a  potential  problem  with  the  Raibert  and  Craig’s  hybrid  approach 
which  uses  the  Jacobian  inverse  for  coordinate  transformations,  but  not  with  the 
stiffness  approach  which  uses  the  Jacobian  transpose.  Although  the  resolved  accel¬ 
eration  method  also  uses  the  Jacobian  inverse,  its  behavior  is  different  because  of 
its  dynamic  compensation.  Further  discussion  of  the  resolved  acceleration  method 
is  postponed  until  the  next  section. 

6.2.1  Hybrid  Control  of  Raibert  and  Craig 

The  equation  for  the  hybrid  controller  (6.1)  is  repeated  below. 


where 


r  =  KpJ^Sx,  +  K„J-1Sxf  +  K/Jr(I  -  S)(frf  -  f) 


x«  =  Xd~  X 
X*  =  X*  -  X 


S  = 


The  first  S  specifies  position  control  in  both  the  x  and  y  directions.  The  second  S 
specifies  position  control  only  in  the  y  direction  and  force  control  in  the  1  direction, 
and  the  reverse  for  the  third  S. 

Let’s  consider  the  cases  when  the  manipulator  is  in  free  space  and  has  no  mass 
at  the  end  of  the  force  sensor  so  that  for  any  axis  selected  to  be  force  controlled, 
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the  desired  and  the  measured  forces  are  0's.  From  the  results  of  Chapter  5,  this 
situation  is  the  most  stable  situation  from  the  dynamic  stability  point  of  view.  Also, 
let’s  look  only  at  the  position  component,  since  the  velocity  component  is  analogous. 
Then, 

r  =  KpJ_1Sx*  «  KpJ-1SJ0,  (6.10) 

where  $t  =  0 4  —  $ 

Also,  let  Kp  =  I  >  0  for  simplicity. 


Case  1  Let  S  = 
Then, 


,  i.e.  position  control  in  both  x  and  y  directions. 


r  =  KpJ-1SJ0t  =  Q,  =  0d-0 


In  this  case,  the  hybrid  controller  becomes  a  simple  independent  joint  controller 
and  the  system  remains  stable  assuming  that  the  control  designer  has  designed  a 
stable  joint  coordinate  control  system. 


Case  2  Let  S 


,  i.e.  position  control  in  y  and  force  control  in  x. 


Since  the  manipulator  is  assumed  to  be  in  free  space,  the  x  axis  force  is  being 
controlled  to  0  force.  With  this  selection  matrix,  position  errors  in  the  x  direction 
should  not  cause  any  restoring  torques.  Expanding  the  simplified  hybrid  controller 
equation, 


r  = 


(611) 


=  J-‘ 


1  ^i2(hci  +  ^12) 

llllSi  (  —  I1S1  —  /j3x2)(/1C1  +  I2C12)  (— ■ fl-Sl  —  /j512)(/2Ci2) 


Let’s  consider  the  configuration  (Fig.6.3)  with  0U  =  0  and  02f  >  0. 


S" 

•  *-"  * 

'A&lkl* 

>!! 


«  0 

0  <  03  <  90 
0U  =  O 
0U>0 


Figure  6.3:  Unstable  configuration  for  hybrid  control 

Although  $u  has  both  x  and  y  components,  the  hybrid  controller  is  being  com¬ 
manded  to  correct  for  the  y-axis  error  only.  Then,  from  (6.12),  the  restoring  torques, 
Ti  and  r2,  are 

rl  =  [^J-(^J«12)(ljCia)]tf2e 

>  0 

T1  —  ~ +  ^2s12)(^2Cl2)|^2« 

=  —  ct(0it$2)0u 

—  <*(01>02)(02  —  &ld) 

<  0 

For  Tj,  the  quantity  inside  the  bracket,  a(0i,02),  is  positive  for  the  configuration 
considered  above.  This  is  a  positive  feedback  on  Joint  2  and  r2  <  0.  Then,  the 
response  for  this  hybrid  control  would  be  as  shown  in  Figure  6.4.  Since  rx  is  posi¬ 
tive  and  r2  is  negative,  the  manipulator  opens  up  and  moves  toward  a  singularity. 
This  movement  will  continue  and  eventually  the  manipulator  will  pass  through  a 
singularity  and  become  unstable. 

Since  the  x-axis  is  being  force  controlled  to  fx  =  0,  and  there  is  no  restoring 
force  to  the  position  error  in  the  x-axis,  this  response  seems  to  be  an  admissible 
response  if  only  looked  from  the  point  of  view  of  cartesian  coordinates.  The  restoring 
torques  are  moving  the  manipulator  to  reduce  the  position  error  in  the  y  direction 
without  any  concern  for  position  error  in  the  x  direction.  However,  in  reality,  this 
is  an  unstable  behavior  since  the  manipulator  is  being  pushed  toward  a  singularity, 
where  the  J-1  matrix  is  undefined. 


Figure  6.4:  Response  of  the  Hybrid  Controller  with  S  = 


0  0 
0  1 


6.2.2  Stiffness  Control  of  Salisbury 

The  potential  instability  problem  of  the  hybrid  controller  does  not  occur  with  the 
stiffness  control  method  of  Salisbury.  As  before,  let’s  look  only  at  the  position  or 
the  stiffness  component  of  the  controller  and  assume  that  the  manipulator  is  in  free 
space.  Then  the  equation  for  the  stiffness  controller  is, 

r  =  3tK3{04  ~0)  =  JtKJ#..  (6.12) 


Assuming  that  the  stiffness  matrix  K  is  diagonal,  the  above  equation  is  expanded 
as 


3tK30 , 


Jn 

J\2 


Jjl 

Jjj 


kx 

0 

J\i 

Jn 

Ou 

0 

ky 

.  J*1 

J22 

02 1 

kx  Jfi  +  kw  J*i  kxJnJi2  +  kvJ2iJjt 

kxJ\\J  12  "f  kyJ2lJ22  kxJ\2  "h  kyJ*2 


0U 

0 2* 

(6.13) 


The  diagonal  entries  of  the  JrKJ  matrix  is  always  positive  for  positive  stiffness 
values.  Let’s  consider  the  same  configuration  as  before,  where  only  the  y-axis  is 
being  position  controlled, 


K  = 


0  0 
0  ky 


0L  =  O,  0j,  >0. 


(6.14) 
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Figure  6.5:  Response  of  the  Stiffness  Controller  with  S  = 


0  0 
0  1 


From  (6.14), 


ri  —  kyJllJ22 

=  kv(liCi  +  /jCu)  +  f2CX2 
>  0 

Tj  =  kyJjl 
>  0 

Unlike  the  case  of  hybrid  control,  the  r2  component  of  the  restoring  torques  is 
positive,  which  is  indicative  of  a  negative  feedback  on  Joint  2.  Then,  as  shown 
in  Figure  6.5,  since  both  rx  and  t2  are  positive,  the  manipulator  corrects  for  the 
error  in  y  position  correctly  and  stably  without  moving  toward  a  singularity.  This 
stable  behavior  can  be  generalized  by  simple  matrix  operations  for  manipulators 
with  more  than  two  joints  and  for  other  manipulator  configurations  and  parts  of 
the  workspace. 


6.2.3  Summary  of  the  intuitive  analytical  results 

The  analyses  presented  in  this  section  suggest  that  the  hybrid  control  method  has 
a  potential  instability  problem  related  to  the  use  of  the  Jacobian  inverse  in  the  co¬ 
ordinate  transformation.  The  Jacobian  transpose  in  stiffness  control  does  not  seem 
to  create  a  similar  problem.  However,  as  stated  at  the  beginning  of  this  section, 
the  above  simple  analyses  are  only  approximate.  I  treated  the  multivariable  ma¬ 
nipulator  system  as  essentially  SISO  systems  and  looked  at  the  responses  due  to  an 
error  in  only  one  component  of  the  state  vector.  But  a  typical  manipulator  is  a  full 
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MIMO  system  with  the  dynamics  of  each  state  coupled  to  one  another.  Therefore, 
one  needs  to  evaluate  the  closed-loop  eigenvalues  of  the  whole  manipulator  system 
under  different  controllers  for  a  more  concrete  stability  analysis. 


6.3  Root  Loci,  Simulations,  and  Experiments  for 
Revolute  Manipulators 


In  this  section,  the  intuition  gained  from  the  previous  approximate  analyses  are 
verified  by  eigenvalue  computations,  simulations,  and  experiments  of  a  revolute 
manipulator  under  different  controllers.  For  computing  the  eigenvalues,  since  a 
revolute  manipulator  is  a  nonlinear  system,  I  can  only  study  local  stability  by 
linearizing  the  dynamics  about  some  equilibrium  points.  The  simulations,  however, 
are  done  using  the  full  nonlinear  dynamics.  The  experimental  results  are  from  the 
implementations  on  the  DDARM  configured  as  a  2-link  planar  manipulator  with 
the  second  joint  locked  at  02  =  180°. 

For  a  planar  manipulator,  the  dynamics  are  given  by, 

r  =  M(0)$  +  b(M)  (6.15) 

Assuming  small  velocities  and  linearizing  the  above  equation  about  some  9  and 
9  =  0,  the  resulting  dynamic  equation  is 

Sr  =  M  (9)69.  (6.16) 


In  state  space  form, 
Sx  = 


0  I 
0  0 


Sx  + 


0 

M(ff)-1 


Sr,  where  Sx  = 


SB 

S9 


(6.17) 


The  inertia  matrix  M(0)  of  a  simple  2-link  revolute  manipulator  (Fig.  6.2)  is 
given  in  (Brady,  et  al.,1982). 


M(0)  = 


Ii  +  I 2  +  m2lili  cos  02  +  +  m2l\)  +  m2l\,  I2  +  \m2l\  +  \m2lil2  cos  02 

h  +  +  \m2hh  cos  02,  I2  +  \m2l\ 


The  inertial  parameters  used  in  the  analyses  are  the  estimated  inertial  parameters 
of  the  DDARM  as  presented  in  Chapter  3  after  converting  the  3-link  parameters  to 


# 


f.H 


those  of  the  simple  2-link  configuration. 

It  =8.095  kg  •  m* 

/j  =0.253  kg  •  m* 
m1=120.1  kg 
m3=2.104  kg 
l\  =0.462  m 
/3  =0.4445  m 

6.3.1  Hybrid  Control 

Let’s  consider  the  stability  of  the  simple  manipulator  under  the  hybrid  control  for 
the  same  configurations  as  in  Section  6.2.1.  At  first,  let’s  consider  the  cases  with  the 
robot  in  free  space  as  before,  which  is  the  most  dynamically  stable  configuration. 
It  will  be  shown  later  that  contacts  do  not  improve  the  kinematic  instabilities. 

In  state  space  form,  the  hybrid  controller  can  be  written  as 


r  1  SO 

St  =  [  -KfJ-1SJ,  — K„J_1SJ  ]  j  . 


(6.18) 


Then,  the  closed-loop  system  is  described  as 


6k  = 


[  — M(0)-1KpJ-1SJ  — M(0)-1KvJ-1SJ 

=  ASx 


(6.19) 


To  guarantee  local  stability  at  the  equilibrium  points,  the  eigenvalues  of  the  A 
matrix  must  have  negative  real  parts. 


Case  1  S  = 


:  Both  the  x  and  y  axes  are  position  controlled.  The 


intuitive  analysis  showed  that  the  hybrid  control  is  stable  in  this  case.  The  root 
locus  diagram  (Fig.  6.6)  shows  the  closed-loop  eigenvalues  of  the  hybrid  controlled 
manipulator  as  0j  varies  from  10°  to  90°  and  9\  =  0°.  As  expected,  the  eigenvalues 
are  on  the  left  hand  side  of  the  s-plane  (LHP)  and  the  manipulator  is  stable. 

The  simulation  (Fig.  6.7)  of  this  case  also  shows  a  stable  behavior.  The  manip¬ 
ulator  is  initially  at  (0i,02)  =  (0°,70°)  and  the  desired  point  is  given  in  cartesian 
coordinates  as  ( x,y )  =  (0.462  m,  0.4445  m).  The  gains  are: 

kp i  =  2500,  kvi  =  300,  kp2  =  400,  kv 2  =  30. 


s  V  *^!**hj 
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Figure  6.7:  Simulation  for  the  hybrid  control  with  S  = 

These  gains  are  chosen  reasonably  and  they  approximately  match  the  actual  gains 
used  in  the  experiments. 

In  the  experimental  results  shown  in  Figure  6.8,  the  geometric  configuration 
of  the  arm  is  180°  rotated  from  the  configuration  shown  on  the  simulation  results 
because  it  was  more  convenient  for  me  to  operate  the  robot  in  this  configuration. 
However,  the  dynamic  behavior  of  the  manipulator  in  either  configuration  is  iden¬ 
tical.  The  results  on  the  direct  drive  arm  also  verify  that  this  case  is  indeed  stable 
for  the  hybrid  control.  Figure  6.8  also  shows  the  manipulator  under  the  hybrid 
controller  executing  a  triangular  trajectory,  defined  by  straight  lines  between  the 
three  corner  points. 


Case  2  S  = 
is  position  control 


0  0 
0  1 


The  x-axis  is  force  controlled  to  0  force  and  the  y-  axis 

ed.  The  root  locus  of  this  case  for  Bx  =  0  and  02  varying  from 
90°  to  70°  is  shown  in  Figure  6.9.  Because  one  axis  is  being  controlled  to  0  force, 
the  behavior  of  the  manipulator  along  that  axis  is  that  of  a  pure  mass  or  a  double 
integrator.  Therefore,  two  of  the  poles  are  at  the  origin,  and  only  the  remaining 
two  poles  are  shown  to  be  varying.  For  02  near  90°,  the  system  is  stable  and  the 
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Figure  6.8:  Experimental  results  for  the  hybrid  control  with  S  = 

eigenvalues  are  negative.  However,  as  $2  becomes  smaller,  the  poles  move  into  the 
right  half  of  the  s-plane.  The  interaction  of  the  inertia  matrix  M(0)  with  the  J_1 
matrix  is  such  that  the  eigenvalues  of  the  A  matrix  of  the  equation  (6.19)  have 
become  positive. 

In  the  above  root  locus,  the  manipulator  was  assumed  to  be  in  free  space.  Figure 
6.10  shows  the  root  locus  for  the  manipulator  in  contact  with  a  stiff  wall  ( k ,  = 
10s  N/m).  Since  the  instability  is  generated  by  the  interaction  of  the  inertia  matrix 
and  the  Jacobian  inverse,  a  manipulator  in  contact  with  its  environment  does  not 
solve  this  instability  problem. 

The  simulations  of  this  case  with  and  without  contact  (Figs.  6.11  and  6.12) 
show  the  expected  unstable  behaviors.  The  experimental  result  is  shown  in  Figure 
6.13  for  the  manipulator  in  free  space.  The  manipulator  is  initially  at  (x,  y)  = 
(—0.462m,  —0.4445m),  which  is  a  stable  configuration  according  to  the  root  locus 
diagram.  Then,  with  a  very  light  force,  I  pulled  the  tip  of  the  manipulator  along  the 
-x  direction,  i.e.  toward  the  more  unstable  configuration.  Then,  the  manipulator 
becomes  unstable  approximately  at  the  point  A  of  the  figure,  which  corresponds  to 
Oj  —  75°  in  the  coordinate  system  used  in  the  root  locus  analyses  and  the  simula¬ 
tions.  This  point  agrees  well  with  the  root  locus  diagram  of  the  ideal  manipulator, 
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Figure  6.9:  Root  locus  for  the  hybrid  control  with  S 


Figure  6.10:  Root  locus  for  the  hybrid  control  with  S  =  q  ^  and  in  contact 
with  a  stiff  wall  ( k ,  =  10s  N/m) 


Figure  6.13:  Experimental  results  for  the  hybrid  control  with  S  = 


0  0 
0  1 


which  shows  that  one  of  the  poles  becomes  positive  at  02  <  79.5°.  Except  for  the 
180°  rotation  in  the  manipulator  configuration  from  that  used  in  the  simulation, 
the  unstable  behavior  in  the  experiment  corresponds  very  closely  with  the  behavior 
in  the  simulation.  For  the  case  of  contact  with  environment,  since  instability  is 
predicted,  I  did  not  try  the  same  experiment  with  the  DDARM  for  fear  of  injuring 
the  robot  and  myself. 


6.3.2  Resolved  Acceleration  Force  Control 

In  state  space  form,  the  linearized  resolved  acceleration  controller  is  written  as 

60 

St  =  [  — M(tf)J-1SKpJ,  — M(0)J-lSK„J  ]  .  .  (6.20) 

L  so 

M(0)  is  the  model  of  the  actual  inertia  matrix  M(0).  Then,  the  closed-loop  system 
is  described  as 

0  I 

-M(0)-1M(0)J-1SKpJ  — M(0)-1M(0)J-1SK„J 


Sx.  (6.21) 
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If  we  assume  perfect  modelling  so  that  M(0)  =  M(0),  the  closed-loop  system  be¬ 


comes 


M  =  ,°  / 

— J-1SKPJ  -J-1SK„J 

=  A5x. 


(6.22) 


This  A  matrix  can  be  decoupled  as 


A  = 


-l  0  0  I  J  0 

>  J"1  — SKp  — SKU  0  J 


=  Q-1BQ. 


(6.23) 


The  above  equation  shows  that  the  A  matrix  for  the  resolved  acceleration  is  a 
similarity  transform  of  the  stable  matrix  B  consisting  only  of  S,  Kp,  and  Kv.  Since 
the  eigenvalues  are  preserved  under  any  similarity  transform,  the  eigenvalues  of  the 
A  matrix  is  simply  given  by  the  eigenvalues  of  B,  which  are  designed  to  be  stable 
by  the  choices  of  Kp  and  K„  matrices.  Therefore,  under  perfect  modelling,  the 
resolved  acceleration  control  results  in  a  stable  system. 

This  is  in  contrast  to  the  earlier  intuitive  analysis  which  seemed  to  suggest 
that  any  method  using  the  Jacobian  inverses  can  lead  to  instability.  However,  as 
I  mentioned  earlier,  that  analysis  was  only  approximate,  and  it  did  not  predict  a 
correct  behavior  for  this  case.  Although  the  Jacobian  inverse  is  still  a  problem  at 
kinematic  singularities,  at  any  other  part  of  manipulator  workspace,  it  does  not 
interact  in  any  harmful  way  with  the  inertia  matrix  since  the  inertia  matrix  is 
cancelled  by  the  resolved  acceleration  controller. 

The  eigenvalues  are  plotted  in  Figure  6.14  for  the  resolved  acceleration  controller 
with  the  following  gains: 


Kp  = 


400  0 


Ku  = 


For  this  controller,  the  eigenvalues  are  negative  and  do  not  vary  with  the  manipu¬ 
lator  configuration  since  the  matrix  B  in  (6.23)  is  independent  of  the  manipulator 
dynamics.  This  stability  property  is  quite  robust  to  the  modelling  errors.  Even 
when  I  added  50%  errors  in  the  inertial  parameters,  the  eigenvalues  remained  in 
the  left  half  plane.  However,  if  the  modelling  errors  are  very  large,  then  the  eigen¬ 
values  will  eventually  become  positive.  For  example,  if  we  model  the  inertia  matrix 
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Figure  6.14:  Root  locus  for  the  resolved  acceleration  control  with  S  = 

as  the  identity  matrix,  this  reduces  to  the  structure  of  the  hybrid  controller  of  Raib- 
ert  and  Craig,  and  the  system  will  become  unstable.  Therefore,  accurate  modelling 
of  the  inertial  terms  of  the  manipulator  is  important  in  force  control  also  as  well 
as  in  trajectory  control.  Since  I  only  evaluated  the  stability  for  the  cases  when  the 
velocity  and  the  gravity  terms  are  0’s,  I  cannot  conclusively  state  how  these  terms 
will  affect  kinematic  stability. 

Since  the  system  is  proven  above  to  be  stable,  simulation  results  are  not  included 
and  only  the  experimental  results  are  presented.  Figure  6.15  shows  the  responses 
when  both  axes  are  position  controlled.  The  response  is  stable  and  for  the  same 
triangular  trajectory  as  before,  this  controller  follows  the  desired  path  much  more 
accurately  than  the  hybrid  controller.  This  is  expected  from  the  trajectory  control 
results  of  Chapter  4  since  the  controller  structure  includes  the  dynamics  of  the 
manipulator.  The  response  when  the  y-axis  is  position  controlled  and  the  x-axis  is 


0  0 
0  1 


force  controlled  to  0  force  is  shown  in  Figure  6.16.  The  manipulator  is  stable  and 
as  desired  only  corrects  for  the  error  in  the  y  direction,  ignoring  the  position  error 
in  the  x  direction. 


6.3.3  Stiffness  Control 


The  stiffness  controller  is  described  in  state  space  form  as, 


6t  =  [  —  JTKPJ,  — JTK„J  ] 


69 
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The  closed-loop  system  is 


Sic 


[  -M{9)~13TKP3  — M (9)~13TKV3 
A6x 


(6.24) 


(6.25) 


Since  dynamics  are  not  included  in  the  controller  structure,  the  inertia  matrix  is 
not  cancelled,  and  it  is  not  obvious  from  the  form  of  the  A  matrix  what  the  stability 
characteristics  should  be.  Therefore,  I  will  follow  the  similar  steps  of  analysis  as  in 
the  study  of  the  hybrid  controller. 


Case  1  Kp  = 


2000  0 
0  2000 


:  The  root  locus  (Fig.  6.17)  for  this  case  shows 


that  the  eigenvalues  are  always  negative  and  the  system  is  stable.  Simulation  results 
are  not  interesting  since  the  system  is  stable  and  are  not  included.  The  experimental 
results  for  this  case  are  shown  in  Figure  6.18.  Since  dynamics  are  not  included, 
the  stiffness  controller  performs  poorly  in  following  the  triangular  path.  Yet,  the 
response  is  always  stable. 


Case  2 


Kp  = 


:  The  root  locus  (Fig.  6.19)  shows  that  this  case  is 


0  0 
0  2000 

also  stable  as  predicted  by  the  intuitive  analysis.  The  experimental  results  shown 
in  Figure  6.20  also  verifies  the  stable  property  of  this  method. 
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Figure  6.17:  Root  locus  for  the  stiffness  control  with  Kp  = 
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Figure  6.18:  Experimental  results  for  the  stiffness  control  with 
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6.4  Stability  for  Polar  Manipulators 

All  of  the  analyses  so  far  have  used  revolute  manipulators,  and  it  was  shown  that  the 
hybrid  control  method  produces  an  unstable  system  for  these  types  of  manipulators. 
This  result,  however,  is  not  true  for  polar  manipulators.  Let’s  consider  a  simple 
planar  polar  manipulator  (Fig.  6.21)  as  described  in  (Lozano-Perez,  1983). 

The  direct  kinematic  relationships  of  such  a  manipulator  are: 


The  Jacobian  relationship  is: 


d2  cos  Q\ 
di  sin  $i 


-d2sin0j  cos 
d2cos0i  sin  0i 


(6.26) 


(6.27) 


The  inverse  Jacobian  relationship  is: 


1  sin  0i  —cos  0i 
d2  — d2cos0!  — d2sin0i 


(6.28) 
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igure  6.19:  Root  locus  for  the  stiffness  control  with  Kp  = 


STIFFNESS:  Kx.«,  Ky«4M 


The  dynamics  are: 


*1 

A 


(dt  -  j/2)Jms  0 
0  m2 


<i2 


+ 


2(d2  -  \l2)m2d20i 

-(dj  -  \li)m20l 


(6.29) 


Intuitive  Analysis  Following  the  intuitive  analysis  for  the  revolute  manipu- 
0  0 


lators,  when  S  = 


0  1 


r  =  KpJ_lSJq« 


1 

d2  cos*  $x 

cos  02  sin  02 

0i# 

2 

d\  sin  0i  cos  02 

d2  sin*  0i 

d2t 

(6.30) 


The  diagonal  elements  of  the  above  gain  matrix  are  always  positive,  resulting  in 
stable  negative  feedbacks  if,  as  before,  the  nonlinear  coupled  system  were  to  be 
approximated  as  two  independent  SISO  systems.  Therefore,  we  would  expect  the 
polar  manipulators  not  to  exhibit  kinematic  instabilities  under  the  hybrid  control. 


Root  locus  and  Simulations  The  stability  analysis  of  a  polar  manipulator 
is  studied  further  by  linearizing  the  dynamics  about  some  equilibrium  points  as 
before.  In  order  to  be  consistent  with  the  results  of  Raibert  and  Craig  (1981),  the 
kinematic,  the  dynamic,  and  the  control  parameters  reported  in  their  study  are  used 
for  evaluating  the  hybrid  controller  by  root  locus  and  simulations  The  root  locus 
with  the  above  S  matrix  is  shown  in  Figure  6.22  as  d2  varies  from  0.05  m  to  0.6  m. 
The  two  non-zero  eigenvalues  are  always  negative  in  this  case.  The  simulation  shown 
in  Figure  6.23  also  demonstrates  a  stable  behavior. 

These  results  on  a  polar  manipulator  verify  the  stable  results  reported  by  Raibert 
and  Craig  in  their  pioneering  implementation  of  hybrid  control  on  the  Stanford  Arm, 
which  is  a  polar  manipulator.  Large  friction  in  their  manipulator  may  have  also 
added  stability  to  their  implementations. 


6.5  Conclusion  on  Kinematic  Stability 


The  kinematic  instabilities  are  very  dependent  on  the  geometric  configuration  of 
and  on  the  types  of  a  manipulator.  The  hybrid  controller  of  Raibert  and  Craig 
produces  an  unstable  system  when  implemented  on  a  revolute  manipulator.  The 
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Figure  6.23:  Simulation  results  for  the  hybrid  control  on  a  polar  manipulator  with 
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same  controller  results  in  a  stable  system  on  a  polar  manipulator.  On  the  other 
hand,  the  stiffness  and  the  resolved  acceleration  methods  always  produce  stable 
results.  The  stiffness  method  is  stable  because  it  uses  the  Jacobian  transpose  for 
coordinate  transformations,  and  the  resolved  acceleration  method  is  stable  because 
the  inertia  matrix  is  included  in  the  controller.  For  cartesian  trajectory  following, 
the  best  performance  was  obtained  with  the  resolved  acceleration  controller  since  its 
inverse  dynamics  compensation  decouples  the  motion  of  the  tip  of  the  manipulator 
in  cartesian  directions. 


In  Chapter  5,  one-link  force  control  experiments  were  included  in  order  to 
demonstrate  the  issues  of  dynamic  stability.  In  Chapter  6,  since  I  wanted  to  isolate 
the  kinematic  instabilities  from  the  dynamic  instabilities,  the  experiments  involved 
force  controllers  for  a  multi-link  manipulator  operating  in  free  space  without  any 
interaction  with  the  environment.  This  chapter  verifies  the  stability  analysis  results 
of  the  two  previous  chapters  by  presenting  results  of  stable  force  controller  imple¬ 
mentations  on  a  multi-link  manipulator  against  a  stiff  environment.  As  in  Chapter 
6,  the  second  joint  was  locked  at  02  =  180°  and  the  first  and  the  third  joints  were 
controlled  to  operate  the  arm  in  a  planar  configuration.  For  all  of  the  experimental 
results  in  this  chapter,  the  environment  was  always  a  stiff  aluminum  surface. 

7.1  Experimental  Setup 

The  resolved  acceleration  method,  which  was  shown  to  be  kinematically  stable  for 
any  manipulator  in  any  configuration,  was  implemented  on  the  two  links  of  the 
direct  drive  arm  to  control  both  the  position  and  the  interaction  force  with  its 
environment.  The  implementation  also  utilized  the  dynamic  stability  results  of 
Chapter  5  by  using  both  joint  torque  control  and  dominant  pole  compensated  wrist 
force  feedback.  The  block  diagram  shown  on  Figure  7.1  and  the  equation  below 
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Figure  7.1:  The  structure  of  the  resolved  acceleration  force/position  controller  for 
the  DDARM 

describe  the  general  structure  of  the  controller. 

r  =  ^(«){M(q)J~l(q)|SxJR— h(q,q)|+b(q,q)+g(q)+Jr(I— S)K/  (f,-f)} 

(7.1) 

Xm  =  +  Kv(x<f  -  x)  +  Kp(Xj  -  x) 

v4(s)  is  the  filter  representing  the  torque  servo  loop  at  each  joint  and  K/  is  the  wrist 
force  sensor  feedback  gain  matrix. 

Joints  1  and  3  have  torque  inner  loops,  operating  at  500 Hz,  and  the  outer  loops 
of  both  force  and  position  operating  at  100 Hz.  Actually  for  Joint  1,  the  inner  loop 
is  really  the  analog  current  loop,  to  which  the  torque  set  points  are  commanded 
at  500 Hz  sampling  intervals.  An  additional  explicit  torque  servo  mentioned  in 
Chapter  5  was  not  necessary  for  this  joint  since  the  commanded  and  the  measured 
torques  matched  quite  well.  However,  since  the  match  was  not  as  good  for  the 
third  joint,  it  has  an  explicit  torque  feedback  in  addition  to  the  current  feedback 
in  order  to  reduce  some  of  the  undesirable  nonlinear  effects  at  the  amplifier/motor 
circuitry.  The  wrist  force  sensor  feedback  was  also  included  in  order  to  increase 
the  steady  state  force  accuracy  at  the  tip  of  the  manipulator.  But  as  discussed  in 


Chapter  5,  to  Insure  stability  by  creating  a  dominant  pole,  the  force  sensor  signal 
is  processed  through  a  low  pass  filter,  whose  pole  is  at  1  Hz.  As  before,  the  Barry 
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Wright  FS6-120A  6-axis  force/torque  sensor  is  used  as  the  wrist  force  sensor. 

The  same  three  types  of  inputs  used  in  the  single  joint  experiments  were  also 
used  in  the  multi-link  experiments.  They  are: 


1.  square  wave  force  in  the  x-axis  and  constant  position  in  the  y-axis, 


2.  sine  wave  force  in  the  x-axis  and  constant  position  in  the  y-axis,  and 


3.  constant  position  in  the  y-axis  and  constant  force  in  the  x-axis  in  presence  of 
the  lateral  movement  of  an  eccentric  cam  (Fig.  5.20). 


As  before,  the  measured  force  data  from  the  wrist  force  sensor  are  plotted  without 
any  low-pass  filtering  of  the  signal. 

The  control  parameters  used  in  the  experiments  are  summarized  below: 


K,  = 


0.444  0 


0  0.444 


KP  = 


160  0 


0  160 


K„  = 


21  0 


0  21 


S  = 


Force  Filter  Pole  =  1  Hz 


The  force  gain  is  set  relatively  low  since  the  main  dynamic  performance  should 
come  from  the  joint  torque  control  part. 


7.2  Experimental  Results 


7.2.1  Square  Wave  Force  Input 


Using  the  resolved  acceleration  method,  the  x-axis  is  force  controlled  with  the  square 
wave  inputs  of  10  N  and  15  N ,  and  the  y-axis  is  position  controlled  to  be  stationary 
at  y  =  —0.669  m.  Figure  7.2  shows  a  stable  and  fast  step  response.  Response 
time,  i.e.,  the  delay  plus  the  rise  time  is  approximately  20ms.  Also,  there  is  very 
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little  coupling  of  the  two  axes,  as  verified  by  the  trace  of  the  error  in  y  position. 
Except  for  some  bias  position  error,  due  to  the  deadzones  in  the  motors  and  also 
probably  due  to  the  roundoff  errors  in  the  computations,  there  is  very  little  change 
in  y  position  as  force  steps  are  commanded  along  the  z-axis. 

For  the  purpose  of  comparison,  the  performance  of  the  resolved  acceleration  force 
controller  without  the  wrist  force  sensor  feedback  is  shown  in  Figure  7.3.  Although 
the  dynamic  charateristics  of  the  step  response  is  similar  to  the  one  with  the  wrist 
force  sensor,  the  steady  state  accuracy  has  deteriorated.  This  result  agrees  with  the 
result  of  the  one  joint  experiments  (Fig.  5.17)  of  Chapter  5. 

7.2.2  Sine  Wave  Force  Input 

Sine  wave  force  response  is  used  as  a  measure  of  bandwidth  for  the  force  controlled 
system.  As  in  the  step  response  experiments,  the  y-axis  is  position  controlled  to  be 
stationary  at  y  =  —0.669  m,  and  the  z-axis  is  force  controlled  with  the  sine  wave 
inputs  of 

fcommand.iN)  =  12.5  +  2.5  Sin(u/f) 

The  force  and  position  responses  to  the  1  Hz  sine  force  input  are  shown  in  Figure 
7.4.  Both  the  force  and  the  position  traces  have  some  offset  errors,  but  the  controller 
performs  well  dynamically.  However,  the  manipulator  did  not  respond  as  well  to 
higher  frequency  inputs,  although  the  one  joint  experiments  showed  good  responses 
up  to  20 Hz  inputs.  This  lower  bandwidth  is  due  to  lower  sampling  frequencies 
(100  Hz  outer  force  loop  vs.  500  Hz)  and  less  rigidity  in  the  manipulator  structure 
caused  by  the  flexibilities  at  the  joints  connecting  the  links. 

7.2.3  Following  the  Eccentric  Cam 

The  manipulator  is  commanded  to  exert  a  constant  force  of  12.5  N  against  the 
surface  of  the  eccentric  cam  (Fig.  5.20)  in  the  z-axis  and  stay  stationary  in  the  y-axis 
at  y  =  —0.669  m.  As  the  cam  turns,  it  produces  a  varying  positional  disturbance 
whose  shape  is  approximately  that  of  a  sine  wave. 

The  performance  under  these  conditions  is  shown  in  Figure  7.5  for  the  same  cam 
speed  used  for  the  one  joint  experiment.  The  plot  of  the  z-axis  position  shows  move¬ 
ments  of  the  the  manipulator  in  the  z  direction,  while  maintaining  a  constant  force 
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Figure  7.2:  Resolved  acceleration:  x-axis=force  steps,  y-axis=constant  position 
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Figure  7.4:  Resolved  acceleration:  x-axis=l  Hz  sine  wave  force,  y-axis=constant 
position 
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against  the  moving  cam.  The  position  trace  in  the  y-axis  shows  that  the  resolved 
acceleration  controller  is  able  to  maintain  constant  y  position  while  following  the 
cam  in  the  x  direction.  Disregarding  the  high  frequency  noise,  the  variation  of  the 
measured  force  is  slightly  larger  than  the  10%  limit  that  I  defined  as  a  disturbance 
rejection  measure.  Therefore,  the  frequency  limit  of  the  two  link  system  measured 
in  this  way  is  slightly  lower  than  0.8  Hz,  which  was  the  limit  of  the  one  joint  system. 


7.3  Some  Results  with  Stiffness  Control 

The  stiffness  method  by  Salisbury  (1980)  was  also  implemented  on  the  direct  drive 
arm  in  order  to  compare  its  performance  against  the  resolved  acceleration  method 
while  the  manipulator  is  in  contact  with  its  environment.  The  gains  for  this  con¬ 
troller  were: 

Fool  „  f  40  0  ] 


KP  = 


0  400 


K„  = 


'If 


Figure  7.5:  Resolved  acceleration:  z-axis=12.5  .AT  force  and  positional  disturbance, 
y-axis=constant  position 


STIFFNESS  CONTROL:  STEP  FORCE  RESPONSE 


Figure  7.6:  Stiffness  Control:  x-axis= force  steps,  y-axis=constant  position 

The  stiffness  in  the  x-axis  is  set  to  0  so  that  pure  force  can  be  controlled  in  this 
direction  in  a  similar  way  as  the  resolved  acceleration  method.  This  is  one  of  two 
modes  of  operation  for  the  stiffness  control.  For  the  other  mode,  one  should  set  the 
stiffness  in  the  z-axis  to  some  desired  value  and  then  command  a  position  set  point 
beyond  the  contact  surface.  Since  only  one  mode  is  evaluated,  the  comparisons  in 
this  section  may  not  be  the  fairest. 

As  before,  the  manipulator  is  commanded  to  maintain  constant  y  position,  and 
square  wave  and  sine  wave  force  commands  are  given  along  z-axis.  The  results  are 
presented  in  Figures  7.6  and  7.7.  Although  the  dynamic  force  responses  are  slightly 
different  from  the  results  of  the  resolved  acceleration,  the  main  differences  are  that 
there  are  significant  movements  in  the  y  direction  with  the  stiffness  control.  Since 
the  manipulator  dynamics  are  not  included  in  the  stiffness  control,  the  coupling 
between  the  two  axes  are  much  more  severe  in  this  cause.  The  results  of  experi¬ 
ments  using  the  cam  positional  disturbance  are  not  presented  because  the  stiffness 
controller  could  not  keep  the  manipulator  tip  on  the  cam  surface.  The  undesirable 
movement  in  the  y  direction  was  greater  than  the  width  of  the  cam. 


Chapter  8 

Conclusion  and  Future  Research 


8.1  Conclusion 

There  are  two  themes  that  are  present  throughout  this  thesis.  One  is  the  importance 
of  accurate  modelling  of  the  manipulator  system  to  be  controlled  and  the  second 
is  the  importance  of  experiments.  Accurate  modelling  of  a  manipulator,  either  by 
estimation  or  by  other  methods,  is  important  for  both  trajectory  and  force  control. 
Experiments  can  not  only  verify  theoretical  results  but  also  give  new  insights  into 
the  problems.  For  example,  through  the  implementation  results,  I  discovered  that 
some  of  the  link  inertial  parameters  cannot  be  estimated  and  also  that  they  do 
not  affect  joint  torques.  Chapter  6  on  kinematic  stability  was  motivated  by  my 
initial  observation  of  instability  when  I  tried  to  implement  hybrid  control  on  the 
DDARM.  These  issues  were  not  apparent  at  first  before  the  experiments.  Therefore, 
these  two  ingredients,  modelling  and  experiments,  are  essential  in  designing  a  high 
performance  controller  for  a  manipulator.  Actually,  they  are  important  for  designing 
a  controller  for  any  system. 

Following  those  themes,  the  work  presented  in  this  thesis  tried  to  address  some 
of  the  problems  in  the  area  of  robot  control  using  the  high  performance  MIT  Se¬ 
rial  Link  Direct  Drive  Arm  as  the  main  experimental  device.  First,  the  issues  of 
estimation  of  the  dynamic  models  of  a  manipulator  and  its  loads  were  studied  since 
accurate  modelling  is  the  first  step  in  any  control  design.  Using  the  algorithms 
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presented,  the  inertial  parameters  of  a  manipulator  and  its  loads  were  estimated 
very  accurately.  The  estimated  dynamic  model  was  then  used  in  evaluating  sev¬ 
eral  well  known  trajectory  control  algorithms  on  the  DDARM.  The  experimental 
evaluations  showed  that  the  manipulator  exhibits  significantly  improved  trajectory 
following  accuracy  when  its  controller  uses  dynamic  compensation. 

The  issues  of  force  control  of  a  manipulator  were  addressed  next.  The  study 
started  with  a  very  simple  model  of  a  manipulator  in  order  to  understand  some  of 
the  dynamic  instability  problems  that  occur  when  a  manipulator  interacts  with  a 
stiff  environment.  After  understanding  the  stability  problems,  several  methods  of 
improving  the  stability  were  suggested  and  evaluated.  It  was  shown  both  analyt¬ 
ically  and  experimentally  that  there  are  also  kinematic  instability  problems  that 
can  occur  even  when  the  manipulator  is  in  free  space.  The  problems  are  caused  by 
the  interaction  of  the  inertia  matrix  with  the  Jacobian  inverses  used  for  kinematic 
coordinate  transformations.  However,  the  kinematic  instabilities  disappear  when  an 
accurate  dynamic  compensation  of  the  manipulator  is  included  in  the  feedback  path 
by  the  resolved  acceleration  algorithm.  This,  then,  brings  us  back  to  the  importance 
of  estimating  the  inertial  parameters  of  a  manipulator  accurately. 

8.2  Future  Research 

The  work  of  this  thesis  is  only  the  beginning  of  growing  interests  in  high  performance 
robot  manipulation.  I  tried  to  address  only  some  of  the  more  imminent  problems 
as  accurate  trajectory  following  and  force  control  stability.  There  are  vast  numbers 
of  difficult  problems  that  have  to  be  answered  in  the  future,  and  many  researchers 
have  already  begun  addressing  them.  Three  problems  that  are  directly  related  to 
this  thesis  work  are  discussed  below. 

8.2.1  Manipulator 

Conventional  highly  geared  robots  can  be  used  successfully  for  applications  that 
do  not  require  high  performance.  It  is  not  able  to  produce  accurate  high  speed 
movements,  nor  interact  with  environments  stably.  Direct  drive  arms  are  high 
performance  robots  that  eliminated  or  reduced  many  of  the  problems  with  the 
conventional  robots.  For  the  next  several  years,  because  of  their  speed,  power,  and 


almost  ideal  dynamic  characteristics,  they  will  continue  to  provide  researchers  with 
many  new  opportunities  to  study  effectively  various  topics  such  as  trajectory  control, 
force  control,  robot  learning,  and  hand  eye  coordination.  However,  as  I  discussed 
in  Chapter  1,  they  also  have  many  shortcomings.  It  is,  therefore,  necessary  to 
consider  new  approaches  to  high  performance  manipulator  design.  One  of  the  new 
approaches  is  a  lightweight  flexible  arm  (Book,  1984;  Cannon  and  Schmitz,  1984). 
This  technology  embodies  a  completely  opposite  philosophy  from  the  technology  of 
a  heavy  and  rigid  direct  drive  arm.  Another  very  exciting  approach  is  a  light  but 
rigid  tendon  driven  arm.  Recently,  this  approach  was  demonstrated  successfully  in 
designing  multi-fingered  robot  hands  (Salisbury,  1982;  Jacobsen  et  al.,  1986). 

8.2.2  Collision 

In  my  study  of  force  control,  I  avoided  the  issues  of  collision  that  occurs  at  the 
initial  contact  phase.  Especially  for  heavy  manipulators,  even  at  low  speeds,  the 
impact  forces  may  be  very  large  such  that  the  environment  and  also  the  manipulator 
itself  are  damaged.  During  the  force  control  experiments  that  I  performed,  although 
the  manipulator  remained  stable,  the  impact  forces  were  typically  greater  than  400% 
of  the  commanded  force  levels  (Fig.  8.1). 

Featherstone  (1984)  developed  a  dynamic  model  of  impact  between  a  robot  and 
its  environment  for  simulation  purposes.  Khatib  and  Burdick  (1986)  introduced  a 
transition  phase  of  pure  damping  control  to  reduce  the  impact  forces  at  collision.  As 
discussed  in  Chapter  5,  adding  a  compliant  skin  to  the  end  effector  will  also  reduce 
the  impact  forces.  Redundant  degrees  of  freedom  may  also  be  used  effectively 
in  handling  collision.  As  humans  bend  knees  when,  for  example,  jumping  down 
from  a  high  platform,  the  robot  can  configure  the  redundant  links  so  that  they  are 
compliant  in  the  direction  of  collision  and  also  transfer  its  momentum  to  other  parts 
of  the  manipulator  structure. 

8.2.3  Fundamental  Concepts  of  Compliant  Behavior 

A  very  important  and  difficult  problem  that  needs  to  be  answered  in  order  to 
improve  significantly  the  compliance  capability  of  a  manipulator  is  the  fundamental 
behavior  we  want  the  robot  to  have  in  interacting  with  its  uncertain  environment. 
It  is  not  clear  whether  we  want  the  manipulator  to  behave  as  a  spring  (Salisbury, 


1980),  a  damper  (Whitney,  1977),  an  impedance  (Hogan,  1985a,  1985b,  1985c),  or 
a  hybrid  of  pure  position/ force  source  (Raibert  and  Craig,  1981,  Khatib,  1983).  It 
may  be  that  none  of  the  above  approaches  is  the  best  one,  and  a  completely  different 
behavior  is  desired. 
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Appendices 


Appendix  1:  Closed  Form  Dynamics 

The  customized  closed  form  equations  of  the  dynamics  of  the  MIT  Serial  Link  Direct 
Drive  Arm  are  presented  in  this  appendix.  To  simplify  the  equations,  the  following 
notation  is  used: 

s2  =  sin'?2»  s3  =  sin  ^3,  c2  =  cos  02,  c3  =  cos03. 

Also,  g  =  9.80665  m/sec2.  The  closed  form  equations  are: 

ni  =  SP 4(^1)  +  SPs(2fli02s2C2  —  ^ic!)  Izyi{20is2c2  —  20\02  40i02C2) 
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+SP2(0!02s2  4"  v*s3c3  —  v*s3c3  —  vjs3c3c2  —  20i02s2c\) 

4-/«ys(t;|  —  vj  4-  vjc2  4-  40\0282s3c3  4-  2t/Jc|  —  2v%c\  —  2v\c\c\) 

4-/*j8(^jS3  -  uJ«js3cj  4-  ^ia2c3  4-  20i#2c2c3) 

+  Iytt  [02CS  ~  ^laJs8  —  20102S3C2  —  Uj52CjC3) 

+  I*xt{03  4-  ^1^232  —  *\c2) 

In  these  equations,  there  are  15  reduced  inertial  parameters:  m2eIa,  ^i(a  >  ^i»ii 
m3cXi,  m3c„s ,  Ixyt,  IyMi,  ItM3,  SPXj  SP2,  SP3,  SP4,  SP3,  SP6.  The  SP,  variables 
are  abbreviations  for  the  following  linear  combinations: 


SPi — m3c,J2  4"  I(«j 
SPj=f«*»  —  lyy* 
SP3=/,jia  4"  /**8 
SP4=f**,  4"  Ixxt  4"  /** 3 


4-  mall 


SP5  —  /tt,  4"  Ixtt  ^ Ml 

SP6=m3C3S  -  m2cn 

Eleven  inertial  parameters  do  not  appear  in  these  equations  at  all,  and  are  com¬ 
pletely  unidentifiable:  m2,  m|CXl,  miCVl}  irijc,,,  Ixyi,  /**,,  Iytl,  m2, 


Vi 


Appendix  2:  Stability  Robustness 


This  appendix  is  a  summary  of  the  discussion  in  Lehtomaki  (1982)  on  the  stabil¬ 
ity  robustness  using  the  Nyquist  criterion  for  a  single  input  single  output  (SISO) 
system, 

Let’s  consider  a  SISO  system  described  in  Figure  A.l  with  nominal  loop  transfer 
function  G(s).  If  we  have  a  modelling  error,  the  actual  loop  transfer  function  is 
represented  as  the  perturbed  G(s).  The  stability  robustness  is  determined  by  the 
distance  that  the  Nyquist  plot  of  G(s)  avoids  the  (-1  ,0)  point  in  the  complex  plane 
(Fig.  A.2).  The  situation  in  Figure  A.2  shows  that  if  the  nominal  closed-loop 
system  with  G(s)  were  stable,  then  the  perturbed  system  with  G(s)  would  also  be 
stable  since  the  number  of  encirclements  of  the  (-1,  0)  point  has  not  changed. 

For  any  w,  the  distance  between  the  (-1,  0)  point  and  G(jw)  is  given  by 

d(w)  =  |1  +  G(jw)  |,  (A.l) 

and  the  distance  between  G(ju)  and  G(ju)  is, 

p(to)  =  | G{ju)  -  G(ju>)|.  (A.2) 


Then,  from  the  Nyquist  plot,  it  is  clear  that  the  perturbed  closed-loop  system  is 
stable  if 


\l  +  G(ju)\  >  |G(jw)  -G(jw)|. 


(A.3) 


G(jw) 


p(w)  7G(jw) 


perturbed  nominal 


Figure  A.2:  Nyquist  plot  of  the  nominal  and  the  perturbed  (actual)  system 

and 

_  G(.)  -  CM 
“  '  G(»)  • 

The  relation  for  stability  robustness  is  obtained  for  this  error  model  by  dividing 
(A.3)  by  G(ju)): 


1  +  G(ju) 
G{jw) 


=  ii + c-'um  >  |c(j^(y^(iM)[ = w«)i.  (a.4) 
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Appendix  3:  Adaptive  Observer 

The  new  plant  equation  (5.20)  can  be  derived  from  the  original  equation  (5.18)  in 
the  following  way.  This  derivation  is  slightly  simpler  than  the  method  presented 
by  Narendra  and  Kudva  (1974).  (5.18)  and  the  filter  equations  (5.19)  are  repeated 
below: 

Dft  =  Xe  +  0XB  +  fiXf;  (5.18) 


y  =  -Ay  +  i  Et  to  =  -A  to  +  f. 

The  above  equations  can  be  written  in  the  frequency  domain  as, 

Df,(s)  =  (sJ  +  08  +  n)x£(s) 

(s  +  A)y(s)  =  X£(s),  (s  +  A)to(s)  =  /,(s). 
Then  by  substituting  (A.6)  into  (A.5), 

(ss  +  0s  +  fl)(s  +  A)y(s)  =  D(s  +  A)io(s). 

Cancelling  (s  +  A)  from  both  sides  of  (A.7),  we  get 

(sa  +  03  +  n)y(s)  =  Dw(s). 


(5.18) 

(5.19) 

(A.5) 

(A.6) 

(A.7) 

(A.8) 


Since  sy(a)  =  xB(s)  —  A y(s)  from  (A.6),  we  can  substitute  for  sy(s)  in  (A.8)  so  that, 
xj s(s)  =  -0xE(s)  -  fly(s)  +  Drv(s)  +  XxB(s)  +  X(0  -  A)y(s)  (A.9) 


In  time  domain,  (A.9)  is  the  desired  equation  (5.20): 

xe  =  ~0XE  -  Hy  +  Dw  +  XxE  4-  X(0  -  A)y 


(5.20) 


Appendix  4:  Operational  Space  and  Resolved  Ac¬ 
celeration 

In  the  reports  by  Khatib  (Khatib,  1983;  Khatib  and  Burdick,  1986),  the  dynamics 
of  a  manipulator  in  operational  space  or  end  effector  cartesian  space  are  described 
by 

A(x)x  +  /i{x,x)  +p(x)  =f.  [I]1 

In  joint  coordinate  system,  the  same  dynamics  are  described  by 

M(q)q  +  b(q,  q)  +  g(q)  =  r.  (A.10) 

M(q)  and  A(x)  are  related  by 

M(q)  =  JT(q)A(x)J(q)  [3] 

or 

J“r(q)M(q)J_1(q)  =  A(x)  =  A(q).  (A.ll) 

Also,  the  operational  space  force  vector  f  and  the  joint  torque  vector  t  are  related 
by 

r  =  Jr(q)f.  [4] 

For  a  decoupled  end  effector  motion  commanded  by  f^, 

t  =  JT(q)A(q)f;  +  Jr/i(x,x)+JTp(x)  m 

=  JT(q)A(q)f^-l-b(q,q)  +g(q). 

where 

b(q,  q)  =  b(q,  q)  -  JrA(q)h(q,  q),  [8] 

h(q,q)  =  j(q)q.  [9] 

Typically,  for  position  or  trajectory  control,  a  linear  second  order  behavior  is  com¬ 
manded  so  that 

C  =  +  IMxj  -  x)  -I-  Kp(x,j  -  x)  (A. 12) 

‘In  this  appendix,  the  equation  numbers  in  brackets  refer  to  the  equation  numbers  in  (Khatib 
and  Burdick,  1986) 


Then,  for  a  hybrid  force/ position  control,  the  joint  command  vector  with  the  oper¬ 
ational  space  method  is 


r  =  JT(q)[i(q)SC  +  (I  -  S)tt  +  b(q,q)  +  g(q)  [18] 

where  is  the  command  vector  for  force  control. 

Substituting  (A.ll)  and  [8]  into  [18], 

t  =  Jr(q)  J_T(q)M(q)J_1(q)Sf^  +  b(q,  q)  -  JTJ-T(q)M(q)J-1(q)h(q,  q)  + 
g(q)+JT(q)(I-S)f*  (A. 13) 

Simplifying  the  above  equation, 

r  =  M(q)J-'(q)[Sf;  -  h(q,q)|  +  b(q,  q)  +  g(q)  +  Jr(q)(I  -  S)C  (A.14) 

This  equation  is  identical  to  the  equation  (6.2)  for  the  modified  resolved  acceleration 
controller  presented  in  Chapters  6  and  7. 
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